def __init__(self):
urdf_path = join(self.path, self.urdf_subpath, self.urdf_filename)
self.model_path = getModelPath(urdf_path, self.verbose)
self.urdf_path = join(self.model_path, urdf_path)
self.robot = RobotWrapper.BuildFromURDF(self.urdf_path, [join(self.model_path, '../..')],
pin.JointModelFreeFlyer() if self.free_flyer else None)
if self.srdf_filename:
self.srdf_path = join(self.model_path, self.path, self.srdf_subpath, self.srdf_filename)
self.robot.q0 = readParamsFromSrdf(self.robot.model, self.srdf_path, self.verbose,
self.has_rotor_parameters, self.ref_posture)
if pin.WITH_HPP_FCL and pin.WITH_HPP_FCL_BINDINGS:
# Add all collision pairs
self.robot.collision_model.addAllCollisionPairs()
# Remove collision pairs per SRDF
pin.removeCollisionPairs(self.robot.model, self.robot.collision_model, self.srdf_path, False)
# Recreate collision data since the collision pairs changed
self.robot.collision_data = self.robot.collision_model.createData()
else:
self.srdf_path = None
self.robot.q0 = pin.neutral(self.robot.model)
if self.free_flyer:
self.addFreeFlyerJointLimits()
def __init__(self):
if self.urdf_filename:
if self.sdf_filename:
raise AttributeError("Please choose between URDF *or* SDF")
df_path = join(self.path, self.urdf_subpath, self.urdf_filename)
builder = RobotWrapper.BuildFromURDF
else:
df_path = join(self.path, self.sdf_subpath, self.sdf_filename)
try:
builder = RobotWrapper.BuildFromSDF
except AttributeError:
raise ImportError("Building SDF models require pinocchio >= 3.0.0")
if self.model_path is None:
self.model_path = getModelPath(df_path, self.verbose)
self.df_path = join(self.model_path, df_path)
self.robot = builder(self.df_path, [join(self.model_path, '../..')],
pin.JointModelFreeFlyer() if self.free_flyer else None)
if self.srdf_filename:
self.srdf_path = join(self.model_path, self.path, self.srdf_subpath, self.srdf_filename)
self.robot.q0 = readParamsFromSrdf(self.robot.model, self.srdf_path, self.verbose,
self.has_rotor_parameters, self.ref_posture)
if pin.WITH_HPP_FCL and pin.WITH_HPP_FCL_BINDINGS:
# Add all collision pairs
self.robot.collision_model.addAllCollisionPairs()
# Remove collision pairs per SRDF
pin.removeCollisionPairs(self.robot.model, self.robot.collision_model, self.srdf_path, False)
# Recreate collision data since the collision pairs changed
self.robot.collision_data = self.robot.collision_model.createData()
else:
self.srdf_path = None
self.robot.q0 = pin.neutral(self.robot.model)
if self.free_flyer:
self.addFreeFlyerJointLimits()
# Load model
model = pin.buildModelFromUrdf(urdf_model_path,pin.JointModelFreeFlyer())
# Load collision geometries
geom_model = pin.buildGeomFromUrdf(model,urdf_model_path,model_path,pin.GeometryType.COLLISION)
# Add collisition pairs
geom_model.addAllCollisionPairs()
print("num collision pairs - initial:",len(geom_model.collisionPairs))
# Remove collision pairs listed in the SRDF file
srdf_filename = "romeo.srdf"
srdf_model_path = model_path + "/romeo_description/srdf/" + srdf_filename
pin.removeCollisionPairs(model,geom_model,srdf_model_path)
print("num collision pairs - final:",len(geom_model.collisionPairs))
# Load reference configuration
pin.loadReferenceConfigurations(model,srdf_model_path)
# Retrieve the half sitting position from the SRDF file
q = model.referenceConfigurations["half_sitting"]
# Create data structures
data = model.createData()
geom_data = pin.GeometryData(geom_model)
# Compute all the collisions
pin.computeCollisions(model,data,geom_model,geom_data,q,False)
if __name__ == "__main__":
# Load the robot urdf.
robot = Solo12Config.buildRobotWrapper()
# Setup the display (connection to gepetto viewer) and load the robot model.
robot.initDisplay(loadModel=True)
# Add collision pairs
robot.collision_model.addAllCollisionPairs()
# Find the absolute path to the srdf file
srdf_path = find_paths("solo12")["srdf"]
# Disable collision pairs specified in the srdf
pin.removeCollisionPairs(robot.model, robot.collision_model, srdf_path)
# Read the reference configurations defined in the srdf
pin.loadReferenceConfigurations(robot.model, srdf_path)
q = robot.model.referenceConfigurations["straight_standing"]
# Display the configuration in the viewer.
robot.display(q)
# Initialize the collision data
robot.collision_data = pin.GeometryData(robot.collision_model)
# Compute all the collisions
pin.computeCollisions(
robot.model,
robot.data,
urdf_model_path = model_path + "/romeo_description/urdf/" + urdf_filename # Load model model = pin.buildModelFromUrdf(urdf_model_path,pin.JointModelFreeFlyer()) # Load collision geometries geom_model = pin.buildGeomFromUrdf(model,urdf_model_path,[model_path],pin.GeometryType.COLLISION) # Add collisition pairs geom_model.addAllCollisionPairs() # Remove collision pairs listed in the SRDF file srdf_filename = "romeo_small.srdf" srdf_model_path = model_path + "/romeo_description/srdf/" + srdf_filename pin.removeCollisionPairs(model,geom_model,srdf_model_path) # Load reference configuration pin.loadReferenceConfigurations(model,srdf_model_path) # Retrieve the half sitting position from the SRDF file q = model.referenceConfigurations["half_sitting"] # Create data structures data = model.createData() geom_data = pin.GeometryData(geom_model) # Compute all the collisions pin.computeCollisions(model,data,geom_model,geom_data,q,False) # Compute for a single pair of collision