def loadModelFromUrdf(self, urdfPath, urdfDir=None, rootJointType=pinocchio.JointModelFreeFlyer, removeMimicJoints=True): """ Load a model using the pinocchio urdf parser. This parser looks for urdf files in which kinematics and dynamics information have been added. - param urdfPath: a path to the URDF file. - param urdfDir: A list of directories. If None, will use ROS_PACKAGE_PATH. """ if urdfPath.startswith("package://"): from os import path n1 = 10 # len("package://") n2 = urdfPath.index(path.sep, n1) pkg = urdfPath[n1:n2] relpath = urdfPath[n2 + 1:] import rospkg rospack = rospkg.RosPack() abspkg = rospack.get_path(pkg) urdfFile = path.join(abspkg, relpath) else: urdfFile = urdfPath if urdfDir is None: import os urdfDir = os.environ["ROS_PACKAGE_PATH"].split(':') if rootJointType is None: self.pinocchioModel = pinocchio.buildModelFromUrdf(urdfFile) else: self.pinocchioModel = pinocchio.buildModelFromUrdf(urdfFile, rootJointType()) self.pinocchioData = pinocchio.Data(self.pinocchioModel) if removeMimicJoints: self._removeMimicJoints(urdfFile=urdfFile)
def _build_from_urdf(self, model_wrapper, urdf_path, package_path): model = model_wrapper.model geom_model = model_wrapper.geom_model pin.buildModelFromUrdf(urdf_path, model) pin.buildGeomFromUrdf( model, urdf_path, pin.GeometryType.COLLISION, geom_model, package_path, ) # viz_model = pin.buildGeomFromUrdf( # model, urdf_path, pin.GeometryType.VISUAL, [packages_path], # ) # ! keep this line here for body dimensions to be defined (otherwize = -inf) model_data = model.createData() geom_model_data = geom_model.createData()
def __init__(self, urdf_path=None): ''' Constructor ''' # Get parameters from the URDF. if urdf_path is None: urdf_path = pkg_resources.resource_filename( 'claptrap_simu', 'data/claptrap.urdf') model = pnc.buildModelFromUrdf(urdf_path) self.g = 9.81 # Gravity self.m = model.inertias[model.getJointId( "BodyJoint")].mass # Mass of the body self.mw = model.inertias[model.getJointId( "WheelJoint")].mass # Mass of the wheel self.I = model.inertias[model.getJointId("BodyJoint")].inertia[ 1, 1] # Body inertia at CoM arong Y self.J = model.inertias[model.getJointId("WheelJoint")].inertia[ 1, 1] # Wheel inertia at CoM around Y self.l = model.inertias[model.getJointId("BodyJoint")].lever[ 2] # Distance of CoM of body from wheel axis self.r = model.jointPlacements[model.getJointId( "BodyJoint")].translation[2] # Radius of the wheel # Define some constants that appear often in the inertia matrix self.mlr = self.m * self.l * self.r self.Mr2 = (self.m + self.mw) * self.r**2 # Motor control law self.motor_control_law = None
def test_self_load(self): hint_list = [self.model_dir] model = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer()) collision_model_ref = pin.buildGeomFromUrdf(model, self.model_path, pin.GeometryType.COLLISION, hint_list) collision_model_self = pin.GeometryModel() pin.buildGeomFromUrdf(model, self.model_path, pin.GeometryType.COLLISION, collision_model_self, hint_list) self.assertTrue(checkGeom(collision_model_ref, collision_model_self)) collision_model_self = pin.GeometryModel() pin.buildGeomFromUrdf(model, self.model_path, pin.GeometryType.COLLISION, collision_model_self, self.model_dir) self.assertTrue(checkGeom(collision_model_ref, collision_model_self)) hint_vec = pin.StdVec_StdString() hint_vec.append(self.model_dir) collision_model_self = pin.GeometryModel() pin.buildGeomFromUrdf(model, self.model_path, pin.GeometryType.COLLISION, collision_model_self, hint_vec) self.assertTrue(checkGeom(collision_model_ref, collision_model_self))
def __init__(self): #Create the object for Abel abel = AbelMove() gesture = AbelGesture() self.model = pinocchio.buildModelFromUrdf( "/home/gabriele/catkin_ws/src/abel_move/scripts/abel_arms_full.urdf" )
def main(): ''' Main procedure 1 ) Build the model from an URDF file 2 ) compute forwardKinematics 3 ) compute forwardKinematics of the frames 4 ) explore data ''' model_file = Path(__file__).absolute().parents[1].joinpath( 'urdf', 'twolinks.urdf') model = buildModelFromUrdf(str(model_file)) # Create data required by the algorithms data = model.createData() # Sample a random configuration numpy_vector_joint_pos = randomConfiguration(model) numpy_vector_joint_vel = np.random.rand(model.njoints - 1) numpy_vector_joint_acc = np.random.rand(model.njoints - 1) # Calls Reverese Newton-Euler algorithm numpy_vector_joint_torques = rnea(model, data, numpy_vector_joint_pos, numpy_vector_joint_vel, numpy_vector_joint_acc) computeRNEADerivatives(model, data, numpy_vector_joint_pos, numpy_vector_joint_vel, numpy_vector_joint_acc) dtorques_dq = data.dtau_dq dtorques_dqd = data.dtau_dv dtorques_dqdd = data.M
def test_deprecated_signatures(self): model = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer()) hint_list = [self.model_dir, "wrong/hint"] collision_model = pin.buildGeomFromUrdf(model, self.model_path, hint_list, pin.GeometryType.COLLISION) hint_vec = pin.StdVec_StdString() hint_vec.append(self.model_dir) collision_model = pin.buildGeomFromUrdf(model, self.model_path, hint_vec, pin.GeometryType.COLLISION) collision_model = pin.buildGeomFromUrdf(model, self.model_path, self.model_dir, pin.GeometryType.COLLISION) if pin.WITH_HPP_FCL_BINDINGS: collision_model = pin.buildGeomFromUrdf(model, self.model_path, hint_list, pin.GeometryType.COLLISION, pin.hppfcl.MeshLoader()) collision_model = pin.buildGeomFromUrdf(model, self.model_path, hint_vec, pin.GeometryType.COLLISION, pin.hppfcl.MeshLoader()) collision_model = pin.buildGeomFromUrdf(model, self.model_path, self.model_dir, pin.GeometryType.COLLISION, pin.hppfcl.MeshLoader())
def main(): ''' Main procedure 1 ) Build the model from an URDF file 2 ) compute forwardKinematics 3 ) compute forwardKinematics of the frames 4 ) explore data ''' model_file = Path(__file__).absolute( ).parents[1].joinpath('urdf', 'twolinks.urdf') model = buildModelFromUrdf(str(model_file)) # Create data required by the algorithms data = model.createData() # Sample a random configuration numpy_vector_joint_pos = randomConfiguration(model) # Perform the forward kinematics over the kinematic tree forwardKinematics(model, data, numpy_vector_joint_pos) # CAUTION updateFramePlacements must be called to update frame's positions # Remark: in pinocchio frames, joints and bodies are different things updateFramePlacements(model, data) # Print out the placement of each joint of the kinematic tree joint_number = model.njoints for i in range(joint_number): joint = model.joints[i] joint_placement = data.oMi[i] frame_number = model.nframes for i in range(frame_number): frame = model.frames[i] frame_placement = data.oMf[i]
def get_manipulator_model(): urdf_filename = pkg_dir + '/data/panda.urdf' model = pin.buildModelFromUrdf(urdf_filename) data = model.createData() print("Created manipulator model: nq={nq}, nv={nv}".format(nq=model.nq, nv=model.nv)) return Manipulator(model, data)
def main(): ''' Main procedure 1 ) Build the model from an URDF file 2 ) compute forwardKinematics 3 ) compute forwardKinematics of the frames 4 ) explore data ''' model_file = Path(__file__).absolute().parents[1].joinpath( 'urdf', 'twolinks.urdf') model = buildModelFromUrdf(str(model_file)) # Create data required by the algorithms data = model.createData() # Sample a random configuration numpy_vector_joint_pos = randomConfiguration(model) numpy_vector_joint_vel = np.random.rand(model.njoints - 1) # Perform the forward kinematics over the kinematic tree forwardKinematics(model, data, numpy_vector_joint_pos, numpy_vector_joint_vel) computeJointJacobians(model, data, numpy_vector_joint_pos) joint_number = model.njoints for i in range(joint_number): joint = model.joints[i] joint_placement = data.oMi[i] joint_velocity = getVelocity(model, data, i, ReferenceFrame.LOCAL_WORLD_ALIGNED) joint_jacobian = getJointJacobian(model, data, i, ReferenceFrame.LOCAL_WORLD_ALIGNED) joint_linear_vel = joint_velocity.linear joint_angular_vel = joint_velocity.angular joint_6v_vel = joint_velocity.vector err = joint_6v_vel - joint_jacobian.dot(numpy_vector_joint_vel) assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err # CAUTION updateFramePlacements must be called to update frame's positions # Remark: in pinocchio frames, joints and bodies are different things updateFramePlacements(model, data) frame_number = model.nframes for i in range(frame_number): frame = model.frames[i] frame_placement = data.oMf[i] frame_velocity = getFrameVelocity(model, data, i, ReferenceFrame.LOCAL_WORLD_ALIGNED) frame_jacobian = getFrameJacobian(model, data, i, ReferenceFrame.LOCAL_WORLD_ALIGNED) frame_linear_vel = frame_velocity.linear frame_angular_vel = frame_velocity.angular frame_6v_vel = frame_velocity.vector err = frame_6v_vel - frame_jacobian.dot(numpy_vector_joint_vel) assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err
def test_load(self): hint_list = [self.model_dir, "wrong/hint"] expected_mesh_path = os.path.join(self.model_dir,'romeo_description/meshes/V1/collision/LHipPitch.dae') model = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer()) collision_model = pin.buildGeomFromUrdf(model, self.model_path, pin.GeometryType.COLLISION, hint_list) col = collision_model.geometryObjects[1] self.assertEqual(col.meshPath, expected_mesh_path)
def __init__(self, urdf_file): self.urdf_filename = urdf_file self.model = pin.buildModelFromUrdf(self.urdf_filename, pin.JointModelFreeFlyer()) self.joint_names = [ "LShoulderPitch", "LShoulderRoll", "LElbowYaw", "LElbowRoll", "LWristYaw", "LHand" ] self.joint_ids = [15, 16, 17, 18, 19, 32]
def addPgTaskToUrdfRobot(robot,solver): # --- ROBOT.PG INIT FRAMES --- robot.geom = Dynamic("geom") if(hasattr(robot, 'jointMap')): for i in robot.jointMap: robot.geom.addJointMapping(i, robot.jointMap[i]) pinocchioModel = se3.buildModelFromUrdf(robot.urdfFile) pinocchioData = pinocchioModel.createData() robot.geom.setModel(pinocchioModel) robot.geom.setData(pinocchioData)
def addPgTaskToUrdfRobot(robot, solver): # --- ROBOT.PG INIT FRAMES --- robot.geom = Dynamic("geom") if (hasattr(robot, 'jointMap')): for i in robot.jointMap: robot.geom.addJointMapping(i, robot.jointMap[i]) pinocchioModel = se3.buildModelFromUrdf(robot.urdfFile) pinocchioData = pinocchioModel.createData() robot.geom.setModel(pinocchioModel) robot.geom.setData(pinocchioData)
def test_multi_load(self): hint_list = [self.model_dir, "wrong/hint"] expected_collision_path = os.path.join( self.model_dir, 'romeo_description/meshes/V1/collision/LHipPitch.dae') expected_visual_path = os.path.join( self.model_dir, 'romeo_description/meshes/V1/visual/LHipPitch.dae') model = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer()) collision_model = pin.buildGeomFromUrdf(model, self.model_path, pin.GeometryType.COLLISION, hint_list) col = collision_model.geometryObjects[1] self.assertEqual(col.meshPath, expected_collision_path) visual_model = pin.buildGeomFromUrdf(model, self.model_path, pin.GeometryType.VISUAL, hint_list) vis = visual_model.geometryObjects[1] self.assertEqual(vis.meshPath, expected_visual_path) model_2, collision_model_2, visual_model_2 = pin.buildModelsFromUrdf( self.model_path, hint_list, pin.JointModelFreeFlyer()) self.assertEqual(model, model_2) col_2 = collision_model_2.geometryObjects[1] self.assertEqual(col_2.meshPath, expected_collision_path) vis_2 = visual_model_2.geometryObjects[1] self.assertEqual(vis_2.meshPath, expected_visual_path) model_c, collision_model_c = pin.buildModelsFromUrdf( self.model_path, hint_list, pin.JointModelFreeFlyer(), geometry_types=pin.GeometryType.COLLISION) self.assertEqual(model, model_c) col_c = collision_model_c.geometryObjects[1] self.assertEqual(col_c.meshPath, expected_collision_path) model_v, visual_model_v = pin.buildModelsFromUrdf( self.model_path, hint_list, pin.JointModelFreeFlyer(), geometry_types=pin.GeometryType.VISUAL) self.assertEqual(model, model_v) vis_v = visual_model_v.geometryObjects[1] self.assertEqual(vis_v.meshPath, expected_visual_path)
def __init__(self, name, device=None, tracer=None): AbstractHumanoidRobot.__init__(self, name, tracer) print "You ask for ", name, ". " rospack = RosPack() self.urdfDir = rospack.get_path('romeo_description') + '/urdf/' if name == 'romeo_small': print "Loaded model is romeo_small.urdf." self.urdfName = 'romeo_small.urdf' self.halfSitting = self.halfSittingSmall else: print "Loaded model is romeo.urdf." self.urdfName = 'romeo.urdf' self.halfSitting = self.halfSittingAll self.OperationalPoints.append('waist') self.OperationalPoints.append('chest') self.OperationalPointsMap = { 'left-wrist': 'LWristPitch', 'right-wrist': 'RWristPitch', 'left-ankle': 'LAnkleRoll', 'right-ankle': 'RAnkleRoll', 'gaze': 'gaze_joint', 'waist': 'waist', 'chest': 'TrunkYaw' } self.device = device # correct the name of the body link self.dynamic = RosRobotModel("{0}_dynamic".format(name)) self.pinocchioModel = se3.buildModelFromUrdf( self.urdfDir + self.urdfName, se3.JointModelFreeFlyer()) self.pinocchioData = self.pinocchioModel.createData() self.dynamic.setModel(self.pinocchioModel) self.dynamic.setData(self.pinocchioData) # complete feet position (TODO: move it into srdf file) #ankle =self.dynamic.getAnklePositionInFootFrame() #self.ankleLength = 0.1935 #self.ankleWidth = 0.121 #self.dynamic.setFootParameters(True , self.ankleLength, self.ankleWidth, ankle) #self.dynamic.setFootParameters(False, self.ankleLength, self.ankleWidth, ankle) # check half sitting size self.dimension = self.dynamic.getDimension() if self.dimension != len(self.halfSitting): raise RuntimeError( "invalid half-sitting pose. HalfSitting Dimension:", len(self.halfSitting), " .Robot Dimension:", self.dimension) self.initializeRobot()
def test_load(self): current_file = os.path.dirname(os.path.abspath(__file__)) model_dir = os.path.abspath(os.path.join(current_file, '../../models/romeo')) model_path = os.path.abspath(os.path.join(model_dir, 'romeo_description/urdf/romeo.urdf')) expected_mesh_path = os.path.join(model_dir,'romeo_description/meshes/V1/collision/LHipPitch.dae') hint_list = [model_dir, "wrong/hint"] model = pin.buildModelFromUrdf(model_path, pin.JointModelFreeFlyer()) collision_model = pin.buildGeomFromUrdf(model, model_path, pin.utils.fromListToVectorOfString(hint_list), pin.GeometryType.COLLISION) col = collision_model.geometryObjects[1] self.assertTrue(col.meshPath == expected_mesh_path)
def main(): ''' Main procedure 1 ) Build the model from an URDF file 2 ) compute forwardKinematics 3 ) compute forwardKinematics of the frames 4 ) explore data ''' model_file = Path(__file__).absolute().parents[1].joinpath( 'urdf', 'twolinks.urdf') model = buildModelFromUrdf(str(model_file)) # Create data required by the algorithms data = model.createData() # Sample a random configuration numpy_vector_joint_pos = randomConfiguration(model) numpy_vector_joint_vel = np.random.rand(model.njoints - 1) numpy_vector_joint_acc = np.random.rand(model.njoints - 1) # Calls Reverese Newton-Euler algorithm numpy_vector_joint_torques = rnea(model, data, numpy_vector_joint_pos, numpy_vector_joint_vel, numpy_vector_joint_acc) # IN WHAT FOLLOWS WE CONFIRM THAT rnea COMPUTES THE FORWARD KINEMATCS computeJointJacobians(model, data, numpy_vector_joint_pos) joint_number = model.njoints for i in range(joint_number): joint = model.joints[i] joint_placement = data.oMi[i] joint_velocity = getVelocity(model, data, i, ReferenceFrame.LOCAL_WORLD_ALIGNED) joint_jacobian = getJointJacobian(model, data, i, ReferenceFrame.LOCAL_WORLD_ALIGNED) err = joint_velocity.vector - \ joint_jacobian.dot(numpy_vector_joint_vel) assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err # CAUTION updateFramePlacements must be called to update frame's positions # Remark: in pinocchio frames, joints and bodies are different things updateFramePlacements(model, data) frame_number = model.nframes for i in range(frame_number): frame = model.frames[i] frame_placement = data.oMf[i] frame_velocity = getFrameVelocity(model, data, i, ReferenceFrame.LOCAL_WORLD_ALIGNED) frame_jacobian = getFrameJacobian(model, data, i, ReferenceFrame.LOCAL_WORLD_ALIGNED) err = frame_velocity.vector - \ frame_jacobian.dot(numpy_vector_joint_vel) assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err
def __init__(self, name, device = None, tracer = None): AbstractHumanoidRobot.__init__ (self, name, tracer) print "You ask for ", name, ". " rospack = RosPack() self.urdfDir = rospack.get_path('romeo_description') + '/urdf/' if name == 'romeo_small': print "Loaded model is romeo_small.urdf." self.urdfName = 'romeo_small.urdf' self.halfSitting = self.halfSittingSmall else: print "Loaded model is romeo.urdf." self.urdfName = 'romeo.urdf' self.halfSitting = self.halfSittingAll self.OperationalPoints.append('waist') self.OperationalPoints.append('chest') self.OperationalPointsMap = {'left-wrist' : 'LWristPitch', 'right-wrist' : 'RWristPitch', 'left-ankle' : 'LAnkleRoll', 'right-ankle' : 'RAnkleRoll', 'gaze' : 'gaze_joint', 'waist' : 'waist', 'chest' : 'TrunkYaw'} self.device = device # correct the name of the body link self.dynamic = RosRobotModel("{0}_dynamic".format(name)) self.pinocchioModel = se3.buildModelFromUrdf(self.urdfDir + self.urdfName, se3.JointModelFreeFlyer()) self.pinocchioData = self.pinocchioModel.createData() self.dynamic.setModel(self.pinocchioModel) self.dynamic.setData(self.pinocchioData) # complete feet position (TODO: move it into srdf file) #ankle =self.dynamic.getAnklePositionInFootFrame() #self.ankleLength = 0.1935 #self.ankleWidth = 0.121 #self.dynamic.setFootParameters(True , self.ankleLength, self.ankleWidth, ankle) #self.dynamic.setFootParameters(False, self.ankleLength, self.ankleWidth, ankle) # check half sitting size self.dimension = self.dynamic.getDimension() if self.dimension != len(self.halfSitting): raise RuntimeError("invalid half-sitting pose. HalfSitting Dimension:", len(self.halfSitting), " .Robot Dimension:", self.dimension) self.initializeRobot()
def test_xml(self): with open(self.model_path) as model: file_content = model.read() model_ref = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer()) model = pin.buildModelFromXML(file_content,pin.JointModelFreeFlyer()) self.assertEqual(model,model_ref) model_self = pin.Model() pin.buildModelFromXML(file_content,pin.JointModelFreeFlyer(),model_self) self.assertEqual(model_self,model_ref)
def test_load(self): current_file = os.path.dirname(os.path.abspath(__file__)) model_dir = os.path.abspath(os.path.join(current_file, '../../models/romeo')) model_path = os.path.abspath(os.path.join(model_dir, 'romeo_description/urdf/romeo.urdf')) expected_mesh_path = os.path.join(model_dir,'romeo_description/meshes/V1/collision/LHipPitch.dae') hint_list = [model_dir, "wrong/hint"] model = pin.buildModelFromUrdf(model_path, pin.JointModelFreeFlyer()) collision_model = pin.buildGeomFromUrdf(model, model_path, pin.utils.fromListToVectorOfString(hint_list), pin.GeometryType.COLLISION) col = collision_model.geometryObjects[1] self.assertTrue(col.meshPath == expected_mesh_path)
def create_pinocchio_model(): pinocchio_model_dir = join(dirname(str(abspath(__file__))), "example_pin_model/") urdf_filename = pinocchio_model_dir + 'model.urdf' # Load the urdf model model = pinocchio.buildModelFromUrdf(urdf_filename) print('model name: ' + model.name) # Create data required by the algorithms data = model.createData() return model, data
def __init__(self, finger_urdf_path: str, tip_link_names: typing.Iterable[str]): """ Args: finger_urdf_path: Path to the URDF file describing the robot. tip_link_names: Names of the finger tip frames, one per finger. """ self.robot_model = pinocchio.buildModelFromUrdf(finger_urdf_path) self.data = self.robot_model.createData() self.tip_link_ids = [ self.robot_model.getFrameId(link_name) for link_name in tip_link_names ]
def __init__(self, finger_urdf_path, tip_link_names): """ Initializes the finger model on which control's to be performed. Args: finger (SimFinger): An instance of the SimFinger class """ self.robot_model = pinocchio.buildModelFromUrdf(finger_urdf_path) self.data = self.robot_model.createData() self.tip_link_ids = [ self.robot_model.getFrameId(link_name) for link_name in tip_link_names ]
def __init__(self, env): self.env = env self.tri_finger_filename = "trifinger_mod.urdf" self.model = pin.buildModelFromUrdf( os.path.join(os.path.dirname(__file__), self.tri_finger_filename)) self.data = self.model.createData() self.kinematics = TriFingerKin(self.model, self.data, self.env) self.params = ModelParams() self.robot_state = None self.end_effectors_id = [4, 8, 12]
def __init__(self): """ Initializes the finger model on which control's to be performed. """ self.urdf_path = '/opt/blmc_ei/src/robot_properties_fingers/urdf/pro/trifingerpro.urdf' self.tip_link_names = [ "finger_tip_link_0", "finger_tip_link_120", "finger_tip_link_240", ] self.robot_model = pinocchio.buildModelFromUrdf(self.urdf_path) self.data = self.robot_model.createData() self.tip_link_ids = [ self.robot_model.getFrameId(link_name) for link_name in self.tip_link_names ]
def __init__(self, name, robotnumber, device=None, tracer=None): AbstractHumanoidRobot.__init__(self, name, tracer) self.OperationalPoints.append('waist') self.OperationalPoints.append('chest') self.device = device self.AdditionalFrames.append( ("accelerometer", matrixToTuple(self.accelerometerPosition), "chest")) self.AdditionalFrames.append( ("gyrometer", matrixToTuple(self.gyrometerPosition), "chest")) self.AdditionalFrames.append( ("leftFootForceSensor", self.forceSensorInLeftAnkle, "left-ankle")) self.AdditionalFrames.append( ("rightFootForceSensor", self.forceSensorInRightAnkle, "right-ankle")) self.OperationalPointsMap = { 'left-wrist': 'LARM_JOINT5', 'right-wrist': 'RARM_JOINT5', 'left-ankle': 'LLEG_JOINT5', 'right-ankle': 'RLEG_JOINT5', 'gaze': 'HEAD_JOINT1', 'waist': 'WAIST', 'chest': 'CHEST_JOINT1' } self.dynamic = RosRobotModel("{0}_dynamic".format(name)) rospack = RosPack() self.urdfPath = rospack.get_path('hrp2_{0}_description'.format( robotnumber)) + '/urdf/hrp2_{0}_reduced.urdf'.format(robotnumber) self.pinocchioModel = se3.buildModelFromUrdf(self.urdfPath, se3.JointModelFreeFlyer()) self.pinocchioData = self.pinocchioModel.createData() self.dynamic.setModel(self.pinocchioModel) self.dynamic.setData(self.pinocchioData) self.dimension = self.dynamic.getDimension() self.plugVelocityFromDevice = True if self.dimension != len(self.halfSitting): raise RuntimeError( "Dimension of half-sitting: {0} differs from dimension of robot: {1}" .format(len(self.halfSitting), self.dimension)) self.initializeRobot() self.dynamic.displayModel()
def __init__(self, name, robotnumber, device = None, tracer = None): AbstractHumanoidRobot.__init__ (self, name, tracer) self.OperationalPoints.append('waist') self.OperationalPoints.append('chest') self.device = device self.AdditionalFrames.append( ("accelerometer", matrixToTuple(self.accelerometerPosition), "chest")) self.AdditionalFrames.append( ("gyrometer", matrixToTuple(self.gyrometerPosition), "chest")) self.AdditionalFrames.append( ("leftFootForceSensor", self.forceSensorInLeftAnkle, "left-ankle")) self.AdditionalFrames.append( ("rightFootForceSensor", self.forceSensorInRightAnkle, "right-ankle")) self.OperationalPointsMap = {'left-wrist' : 'LARM_JOINT5', 'right-wrist' : 'RARM_JOINT5', 'left-ankle' : 'LLEG_JOINT5', 'right-ankle' : 'RLEG_JOINT5', 'gaze' : 'HEAD_JOINT1', 'waist' : 'WAIST', 'chest' : 'CHEST_JOINT1'} self.dynamic = RosRobotModel("{0}_dynamic".format(name)) rospack = RosPack() self.urdfPath = rospack.get_path('hrp2_{0}_description'.format(robotnumber)) + '/urdf/hrp2_{0}.urdf'.format(robotnumber) self.pinocchioModel = se3.buildModelFromUrdf(self.urdfPath, se3.JointModelFreeFlyer()) self.pinocchioData = self.pinocchioModel.createData() self.dynamic.setModel(self.pinocchioModel) self.dynamic.setData(self.pinocchioData) self.dimension = self.dynamic.getDimension() self.plugVelocityFromDevice = True if self.dimension != len(self.halfSitting): raise RuntimeError("Dimension of half-sitting: {0} differs from dimension of robot: {1}".format (len(self.halfSitting), self.dimension)) self.initializeRobot() self.dynamic.displayModel()
def main(): """ Main procedure 1 ) Get the path of the urdf model 2 ) Build the pinocchio model 2 ) Print some infor of the model """ model_file = Path(__file__).absolute().parents[1].joinpath( 'urdf', 'twolinks.urdf') # Load the urdf model model = buildModelFromUrdf(str(model_file)) model_name = model.name torque_limits_of_motors = model.effortLimit velocity_limits_of_motors = model.velocityLimit number_of_joints = model.njoints for i in range(number_of_joints): joint = model.joints[i] joint_name = model.names[i] jid = joint.idx_q joint_placement = model.jointPlacements[i] numpy_translation_vector = joint_placement.translation numpy_rotation_matrix = joint_placement.rotation if jid >= 0: damping = model.damping[jid] effortLimit = model.effortLimit[jid] friction = model.friction[jid] lowerPositionLimit = model.lowerPositionLimit[jid] upperPositionLimit = model.upperPositionLimit[jid] velocityLimit = model.velocityLimit[jid] number_of_frames = model.nframes for i in range(number_of_frames): frame = model.frames[i] frame_name = frame.name frame_placement = frame.placement numpy_translation_vector = frame_placement.translation numpy_rotation_matrix = frame_placement.rotation # import pdb # pdb.set_trace() for inertia in model.inertias: print(inertia)
def test_pickle(self): import pickle model_dir = os.path.abspath( os.path.join(self.current_file, "../../models/example-robot-data/robots")) model_path = os.path.abspath( os.path.join(model_dir, "ur_description/urdf/ur5_robot.urdf")) model = pin.buildModelFromUrdf(model_path) filename = "model.pickle" with open(filename, 'wb') as f: pickle.dump(model, f) with open(filename, 'rb') as f: model_copy = pickle.load(f) self.assertTrue(model == model_copy)
def talker(): rospy.init_node("tocabi_pinocchio", anonymous = False) rospy.Subscriber("/tocabi/pinocchio/jointstates", JointState, callback) global model pub = rospy.Publisher("/tocabi/pinocchio", model, queue_size=1) modelmsg.CMM = [0 for i in range(33*6)] modelmsg.COR = [0 for i in range(33*33)] modelmsg.M = [0 for i in range(33*33)] modelmsg.g = [0 for i in range(33)] rate = rospy.Rate(1000) #10hz model = pinocchio.buildModelFromUrdf("/home/jhk/catkin_ws/src/dyros_tocabi/tocabi_description/robots/dyros_tocabi.urdf",pinocchio.JointModelFreeFlyer()) data = model.createData() global start q = pinocchio.randomConfiguration(model) qdot = pinocchio.utils.zero(model.nv) qdot_t = pinocchio.utils.zero(model.nv) qddot_t = pinocchio.utils.zero(model.nv) while not rospy.is_shutdown(): if start: global qmsg for i in range(0, 39): q[i] = qmsg.position[i] for j in range(0, 38): qdot[j] = qmsg.velocity[j] qdot_t[j] = 0.0 qddot_t[j] = 0.0 CMM = pinocchio.computeCentroidalMap(model, data, q) pinocchio.crba(model, data, q) pinocchio.computeCoriolisMatrix(model, data, q, qdot) pinocchio.rnea(model, data, q, qdot_t, qddot_t) CMM_slice = CMM[:,6:39] COR_slice = data.C[:,6:39] M_slice = data.M[:,6:39] g_slice = data.tau[6:39] modelmsg.CMM = CMM_slice.flatten() modelmsg.COR = COR_slice.flatten() modelmsg.M = M_slice.flatten() modelmsg.g = g_slice.flatten()
def check_offset_to_pinocchio_model_in_cart_space(self, q): import pinocchio from classic_framework.utils.geometric_transformation import mat2quat obj_urdf = sim_framework_path("./envs/panda_arm_hand_pinocchio.urdf") model = pinocchio.buildModelFromUrdf(obj_urdf) data = model.createData() q_pin = np.zeros(9) q_pin[:7] = q pinocchio.framesForwardKinematics(model, data, q_pin) pinocchio.computeJointJacobians(model, data, q_pin) pinocchio.framesForwardKinematics(model, data, q_pin) # get orientation rotation_matrix = data.oMf[model.getFrameId('panda_grasptarget')].rotation quaternions = mat2quat(rotation_matrix) # [w x y z] jac = pinocchio.getFrameJacobian(model, data, model.getFrameId('panda_grasptarget'), pinocchio.LOCAL_WORLD_ALIGNED)[:, :7] print('position offset:', self.current_c_pos - np.array(data.oMf[model.getFrameId('panda_grasptarget')].translation))
def __init__(self, init_j_pos, dt, offset=0, config_path=None): if config_path is None: config_path = sim_framework_path('./classic_framework/controllers/Config/PyBullet/FictiveRobot') super().__init__(config_path, dt=dt) obj_urdf = sim_framework_path("./envs/panda_arm_hand_pinocchio.urdf") self.model = pinocchio.buildModelFromUrdf(obj_urdf) self.data = self.model.createData() # The offset is [0, 0, 0.88] for pybullet (with the table) # The offset is [0, 0, 0] for mujoco (with current scene) # NOTE: adjust the offset (z-direction), if you re-place the robot! (Use check offset function of fictive robot) self.offset = offset self.init_j_pos = init_j_pos self.end_effector_frame_id = self.model.getFrameId('panda_grasptarget') self.des_joint_traj = [] self.gotoJointController = GotoJointController(self.dt) self.gotoCartPosController = GotoCartPosImpedanceController(self.dt) self.gotoCartPosQuatController = GotoCartPosQuatImpedanceController(self.dt) self.jointTrajectoryTracker = JointTrajectoryTracker(self.dt)
import pinocchio as pin from pinocchio.romeo_wrapper import RomeoWrapper import os current_path = os.getcwd() # The model of Romeo is contained in the path PINOCCHIO_GIT_REPOSITORY/models/romeo model_path = current_path + "/" + "../../models/romeo" mesh_dir = model_path urdf_filename = "romeo_small.urdf" 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
0.0, 0.1, -0.005, # Right Arm 0., 0. # Head ] q = np.matrix(halfSitting).T print("q:") print(q.flatten().tolist()[0]) rospack = RosPack() urdfPath = rospack.get_path('talos_data') + "/urdf/talos_reduced.urdf" urdfDir = [rospack.get_path('talos_data') + "/../"] model = pin.buildModelFromUrdf(urdfPath, pin.JointModelFreeFlyer()) data = model.createData() com = pin.centerOfMass(model, data, q) pin.updateFramePlacements(model, data) m = data.mass[0] h = float(com[2]) g = 9.81 omega = sqrt(g / h) leftName = param_server_conf.footFrameNames['Left'] leftId = model.getFrameId(leftName) leftPos = data.oMf[leftId] rightName = param_server_conf.footFrameNames['Right'] rightId = model.getFrameId(rightName) rightPos = data.oMf[rightId]
import pinocchio from sys import argv filename = "ur5.urdf" if len(argv)<2 else argv[1] model = pinocchio.buildModelFromUrdf(filename) data = model.createData() q = pinocchio.randomConfiguration(model) print 'q = ', q.T pinocchio.forwardKinematics(model,data,q) for k in range(model.njoints): print("{:<24} : {: .2f} {: .2f} {: .2f}" .format( model.names[k], *data.oMi[k].translation.T.flat ))