Commit 229db2c5 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

cleaning + filter on roms

parent 4bd41aec
......@@ -70,6 +70,7 @@ install(FILES
data/urdf/truck.urdf
data/urdf/stair_bauzil.urdf
data/urdf/climb.urdf
data/urdf/stepladder.urdf
#~ data/urdf/scene2.urdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf
)
......@@ -83,6 +84,7 @@ install(FILES
data/srdf/truck.srdf
data/srdf/stair_bauzil.srdf
data/srdf/climb.srdf
data/srdf/stepladder.srdf
#~ data/srdf/scene2.srdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/srdf
)
......@@ -101,6 +103,7 @@ install(FILES
data/meshes/truck_vision.stl
data/meshes/stair_bauzil.stl
data/meshes/climb.stl
data/meshes/stepladder.stl
#~ data/meshes/pedal.stl
#~ data/meshes/board.stl
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes
......
......@@ -27,7 +27,7 @@ module hpp
interface RbprmBuilder
{
/// Create a Device for the ROM of the robot
/// This function can be called several times to include several ROMs (one for each limb)
/// The device automatically has an anchor joint called "base_joint" as
/// root joint.
/// \param romRobotName the name of the robot range of motion.
......@@ -87,19 +87,81 @@ module hpp
in string urdfSuffix, in string srdfSuffix)
raises (Error);
/// Get Sample configuration
/// \return dofArray Array of degrees of freedom.
/// Set Rom constraints for the configuration shooter
/// a configuration will only be valid if all roms indicated
/// are colliding with the environment.
/// If no roms are indicated, a configuration will be valid if any rom
/// is colliding with the environment.
///
void setFilter (in Names_t roms)
raises (Error);
/// Get Sample configuration by its id
/// \param sampleName name of the limb from which to retrieve a sample
/// \param sampleId id of the desired samples
/// \return dofArray Array of degrees of freedom corresponding to the current configuration of the robot
/// , to which the desired limb configuration has been assigned.
floatSeq getSampleConfig(in string sampleName, in unsigned short sampleId) raises (Error);
/// Get the end effector position of a given limb configuration
/// \param sampleName name of the limb from which to retrieve a sample
/// \param sampleId id of the desired samples
/// \return world position of the limb end effector given the current robot configuration and the
/// and the selected sample
floatSeq getSamplePosition(in string sampleName, in unsigned short sampleId) raises (Error);
/// Generate all possible contact in a given configuration
/// \param dofArray initial configuration of the robot
/// \param direction desired direction of motion for the robot
/// \return transformed configuration for which all possible contacts have been created
floatSeq generateContacts(in floatSeq dofArray, in floatSeq direction) raises (Error);
/// Given a configuration and a limb, returns the id of all samples potentially in contact with the
/// environment, ordered by their efficiency
/// \param name name of the considered limb
/// \param dofArray considered configuration of the robot
/// \param direction desired direction of motion for the robot
/// \return transformed configuration for which all possible contacts have been created
floatSeq getContactSamplesIds(in string name, in floatSeq dofArray, in floatSeq direction) raises (Error);
/// Specifies a subchain of the robot as a limb, which can be used to create contacts.
/// A limb must consist in a simple kinematic chain, where every node has only one child
/// \param id user given name of the new limb
/// \param limb robot joint corresponding to the root of the limb (ex a shoulder or ankle joint)
/// \param effector robot joint corresponding to the effector of the limb (ex a hand or foot joint)
/// \param offset contact point of the effector, expressed as an offset from the joint root
/// \param normal normal vector to consider for contact creation. For instance for a foot, typically
/// normal is aligned with the -z vertical axis, to create a contact with the plant of the robot
/// \param x width of the rectangle surface contact of the effector
/// \param y length of the rectangle surface contact of the effector
/// \param samples number of samples to generate for the limb (a typical value is 10000)
/// \param resolution resolution of the octree used to store the samples (a typical value is 0.01 meters)
void addLimb(in string id, in string limb, in string effector, in floatSeq offset, in floatSeq normal,
in double x, in double y, in unsigned short samples, in double resolution) raises (Error);
/// Set the start state of a contact generation problem
/// environment, ordered by their efficiency
/// \param dofArray start configuration of the robot
/// \param contactLimbs ids of the limb in contact for the state
void setStartState(in floatSeq dofArray, in Names_t contactLimbs) raises (Error);
/// Set the end state of a contact generation problem
/// environment, ordered by their efficiency
/// \param dofArray end configuration of the robot
/// \param contactLimbs ids of the limb in contact for the state
void setEndState(in floatSeq dofArray, in Names_t contactLimbs) raises (Error);
/// Provided a path has already been computed, interpolates it and generates the statically stable
/// constact configurations along it. setStartState and setEndState must have been called prior
/// to this function. If these conditions are not met an error is raised.
/// \param timestep normalized step for generation along the path (ie the path has a length of 1).
floatSeqSeq interpolate(in double timestep) raises (Error);
/// Saves the last computed states by the function interpolate in a filename.
/// Raises an error if interpolate has not been called, or the file could not be opened.
/// \param filename name of the file used to save the contacts.
void saveComputedStates(in string filename) raises (Error);
}; // interface Robot
......
#!/usr/bin/env python
import pygame
import time
pygame.init()
pygame.joystick.init()
pygame.event.pump()
if (pygame.joystick.get_count() > 0):
print "found gamepad! : " + pygame.joystick.Joystick(0).get_name()
my_joystick = pygame.joystick.Joystick(0)
my_joystick.init()
for i in range(100):
pygame.event.pump()
time.sleep(0.1)
x=my_joystick.get_axis(0)
print x
......@@ -18,7 +18,8 @@ srdfSuffix = ""
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [0,4, -1, 1, 1, 2.5])
#~ fullBody.setJointBounds ("base_joint_xyz", [0,4, -1, 1, 1, 2.5])
fullBody.setJointBounds ("base_joint_xyz", [-1, 1, -4, 0, 1, 2.5])
#~ from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
......@@ -39,14 +40,14 @@ rLeg = 'RLEG_JOINT0'
rLegOffset = [0,-0.105,0,]
rLegNormal = [0,1,0]
rLegx = 0.09; rLegy = 0.05
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 20000, 0.03)
#~ fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 20000, 0.03)
lLegId = '8lLeg'
lLeg = 'LLEG_JOINT0'
lLegOffset = [0,-0.105,0]
lLegNormal = [0,1,0]
lLegx = 0.09; lLegy = 0.05
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 20000, 0.03)
#~ fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 20000, 0.03)
rarmId = '3Rarm'
rarm = 'RARM_JOINT0'
......@@ -66,21 +67,21 @@ lArmNormal = [1,0,0]
lArmx = 0.024; lArmy = 0.024
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 20000, 0.01)
#~ rLegId = '5RKnee'
#~ rLeg = 'RLEG_JOINT0'
#~ rKnee = 'RLEG_JOINT3'
#~ rLegOffset = [0.105,0.055,0.017]
#~ rLegNormal = [-1,0,0]
#~ rLegx = 0.05; rLegy = 0.05
#~ fullBody.addLimb(rLegId, rLeg,rKnee,rLegOffset,rLegNormal, rLegx, rLegy, 5000, 0.01)
#~ lLegId = '6LKnee'
#~ lLeg = 'LLEG_JOINT0'
#~ lKnee = 'LLEG_JOINT3'
#~ lLegOffset = [0.105,0.055,0.017]
#~ lLegNormal = [-1,0,0]
#~ lLegx = 0.05; lLegy = 0.05
#~ fullBody.addLimb(lLegId,lLeg,lKnee,lLegOffset,lLegNormal, lLegx, lLegy, 5000, 0.01)
rLegId = '5RKnee'
rLeg = 'RLEG_JOINT0'
rKnee = 'RLEG_JOINT3'
rLegOffset = [0.105,0.055,0.017]
rLegNormal = [-1,0,0]
rLegx = 0.05; rLegy = 0.05
fullBody.addLimb(rLegId, rLeg,rKnee,rLegOffset,rLegNormal, rLegx, rLegy, 20000, 0.01)
lLegId = '6LKnee'
lLeg = 'LLEG_JOINT0'
lKnee = 'LLEG_JOINT3'
lLegOffset = [0.105,0.055,0.017]
lLegNormal = [-1,0,0]
lLegx = 0.05; lLegy = 0.05
fullBody.addLimb(lLegId,lLeg,lKnee,lLegOffset,lLegNormal, lLegx, lLegy, 20000, 0.01)
#~
......
......@@ -73,7 +73,9 @@ def test_inequality():
else:
print("test of inequality successful")
obj = load_obj('../data/roms/comlArmSimplified.obj')
ineq = asInequalities(obj)
print(is_inside(ineq, [0,0,0]))
print(is_inside(ineq, [4,0,0]))
#~ obj = load_obj('../data/roms/comlArmSimplified.obj')
#~ ineq = asInequalities(obj)
#~ print(is_inside(ineq, [0,0,0])) # in
#~ print(is_inside(ineq, [0.873,-0.03551,-0.1630])) #out
#~ print(is_inside(ineq, [0.6730,-0.03551,-0.1630])) #in
#~ print(is_inside(ineq, [-0.08897,0.7172,-0.2494])) #in
......@@ -4,7 +4,7 @@ from hpp.gepetto import Viewer
rootJointType = 'freeflyer'
packageName = 'hpp-rbprm-corba'
meshPackageName = 'hpp-rbprm-corba'
urdfName = 'hrp2_trunk'
urdfName = 'hrp2_trunk08'
urdfNameRom = 'hrp2_rom'
urdfSuffix = ""
srdfSuffix = ""
......@@ -12,7 +12,7 @@ srdfSuffix = ""
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [0,4, -1, 1, 1, 2.5])
rbprmBuilder.setJointBounds ("base_joint_xyz", [-1, 1, -4, 0, 1, 2.5])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
......@@ -23,18 +23,21 @@ r = Viewer (ps)
q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
#~ fullBody.client.basic.robot.setJointConfig('base_joint_SO3',[0.7316888688738209, 0, -0.6816387600233341, 0]); q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
#~ rbprmBuilder.client.basic.robot.setJointConfig('base_joint_SO3',[0.7316888688738209, 0, -0.6816387600233341, 0]); q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
q_init [0:6] = [0.0, -2.2, 2.0, 0.7316888688738209, 0.0, 0.6816387600233341];
q_init = [2.5, -0.1, 1.55, 1.0, 0.0, 0.0, 0.0]; r (q_init) # 1 scale
q_init = [0.0, -2.2, 2.0, 0.53536860083385163, -0.46463139916614854, 0.49874749330202722, 0.49874749330202722];
q_init = [-0.1, -2.65, 2.1, 0.53536860083385163, 0.46463139916614854, 0.49874749330202722, -0.49874749330202722];r(q_init)
q_init = [-0.1, -2.4, 1.9, 0.53536860083385163, 0.46463139916614854, 0.49874749330202722, -0.49874749330202722];r(q_init)
#~ q_init = [2.5, -0.1, 1.55, 1.0, 0.0, 0.0, 0.0]; r (q_init) # 1 scale
#~ q_init [0:3] = [2.2, 0, 2]; r (q_init)
#~ q_init [0:3] = [2., -0.1, 1.8]; r (q_init) # 0.9 scale
#~ q_init [0:3] = [2., -0.1, 1.6]; r (q_init) # 0.8 scale
rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_goal = [4.0, -0.1, 1.8, 1.0, 0.0, 0.0, 0.0]
q_goal = [0, -4.0, 1.8, 0.7316888688738209, 0.0, 0.0, -0.68163876002333412]
#~ q_goal [0:3] = [4.0, 0, 2.0]; r (q_goal) # 1 scale
#~ q_goal [0:3] = [3., -0.1, 1.9]; r (q_goal) # 0.9 scale
#~ q_goal [0:3] = [3., -0.1, 1.5]; r (q_goal) # 0.8 scale
......
......@@ -81,7 +81,13 @@ class Builder (object):
return self.client.rbprm.rbprm.initshooter ()
## \}
## Init RbprmShooter
#
# \param jointName name of the joint,
# \return name of the link.
def setFilter (self, romFilter):
return self.client.rbprm.rbprm.setFilter (romFilter)
## \}
## \name Degrees of freedom
# \{
......
......@@ -52,8 +52,9 @@ namespace hpp {
{
try
{
romDevice_ = model::Device::create (robotName);
hpp::model::urdf::loadRobotModel (romDevice_,
hpp::model::DevicePtr_t romDevice = model::Device::create (robotName);
romDevices_.insert(std::make_pair(robotName, romDevice));
hpp::model::urdf::loadRobotModel (romDevice,
std::string (rootJointType),
std::string (packageName),
std::string (modelName),
......@@ -83,7 +84,7 @@ namespace hpp {
}
try
{
hpp::model::RbPrmDevicePtr_t device = hpp::model::RbPrmDevice::create (robotName, romDevice_);
hpp::model::RbPrmDevicePtr_t device = hpp::model::RbPrmDevice::create (robotName, romDevices_);
hpp::model::urdf::loadRobotModel (device,
std::string (rootJointType),
std::string (packageName),
......@@ -133,7 +134,6 @@ namespace hpp {
fullBodyLoaded_ = true;
}
hpp::floatSeq* RbprmBuilder::getSampleConfig(const char* limb, unsigned short sampleId) throw (hpp::Error)
{
if(!fullBodyLoaded_)
......@@ -224,6 +224,12 @@ namespace hpp {
return res;
}
void RbprmBuilder::setFilter(const hpp::Names_t& roms) throw (hpp::Error)
{
bindShooter_.romFilter_ = stringConversion(roms);
}
hpp::floatSeq* RbprmBuilder::generateContacts(const hpp::floatSeq& configuration, const hpp::floatSeq& direction) throw (hpp::Error)
{
if(!fullBodyLoaded_)
......
......@@ -41,9 +41,10 @@ namespace hpp {
{
hpp::model::RbPrmDevicePtr_t robotcast = boost::static_pointer_cast<hpp::model::RbPrmDevice>(robot);
return hpp::rbprm::RbPrmShooter::create
(robotcast,problemSolver_->problem ()->collisionObstacles(),shootLimit_,displacementLimit_);
(robotcast,problemSolver_->problem ()->collisionObstacles(),romFilter_,shootLimit_,displacementLimit_);
}
hpp::core::ProblemSolverPtr_t problemSolver_;
std::vector<std::string> romFilter_;
std::size_t shootLimit_;
std::size_t displacementLimit_;
};
......@@ -75,6 +76,8 @@ namespace hpp {
const char* urdfSuffix,
const char* srdfSuffix) throw (hpp::Error);
virtual void setFilter(const hpp::Names_t& roms) throw (hpp::Error);
virtual hpp::floatSeq* getSampleConfig(const char* limb, unsigned short sampleId) throw (hpp::Error);
virtual hpp::floatSeq* getSamplePosition(const char* limb, unsigned short sampleId) throw (hpp::Error);
......@@ -100,7 +103,7 @@ namespace hpp {
core::ProblemSolverPtr_t problemSolver_;
private:
model::DevicePtr_t romDevice_;
model::T_Rom romDevices_;
rbprm::RbPrmFullBodyPtr_t fullBody_;
bool romLoaded_;
bool fullBodyLoaded_;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment