geometry-models.py
Go to the documentation of this file.
1 import pinocchio
2 from sys import argv
3 from os.path import dirname, join, abspath
4 
5 # This path refers to Pinocchio source code but you can define your own directory here.
6 pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models")
7 
8 model_path = join(pinocchio_model_dir,"example-robot-data/robots") if len(argv)<2 else argv[1]
9 mesh_dir = pinocchio_model_dir
10 urdf_model_path = join(model_path,"ur_description/urdf/ur5_robot.urdf")
11 
12 # Load the urdf model
13 model, collision_model, visual_model = pinocchio.buildModelsFromUrdf(urdf_model_path, mesh_dir)
14 print('model name: ' + model.name)
15 
16 # Create data required by the algorithms
17 data, collision_data, visual_data = pinocchio.createDatas(model, collision_model, visual_model)
18 
19 # Sample a random configuration
21 print('q: %s' % q.T)
22 
23 # Perform the forward kinematics over the kinematic tree
24 pinocchio.forwardKinematics(model,data,q)
25 
26 # Update Geometry models
27 pinocchio.updateGeometryPlacements(model, data, collision_model, collision_data)
28 pinocchio.updateGeometryPlacements(model, data, visual_model, visual_data)
29 
30 # Print out the placement of each joint of the kinematic tree
31 print("\nJoint placements:")
32 for name, oMi in zip(model.names, data.oMi):
33  print(("{:<24} : {: .2f} {: .2f} {: .2f}"
34  .format( name, *oMi.translation.T.flat )))
35 
36 # Print out the placement of each collision geometry object
37 print("\nCollision object placements:")
38 for k, oMg in enumerate(collision_data.oMg):
39  print(("{:d} : {: .2f} {: .2f} {: .2f}"
40  .format( k, *oMg.translation.T.flat )))
41 
42 # Print out the placement of each visual geometry object
43 print("\nVisual object placements:")
44 for k, oMg in enumerate(visual_data.oMg):
45  print(("{:d} : {: .2f} {: .2f} {: .2f}"
46  .format( k, *oMg.translation.T.flat )))
void updateGeometryPlacements(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const GeometryModel &geom_model, GeometryData &geom_data)
Update the placement of the geometry objects according to the current joint placements contained in d...
void forwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &a)
Update the joint placements, spatial velocities and spatial accelerations according to the current jo...
void randomConfiguration(const LieGroupGenericTpl< LieGroupCollection > &lg, const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Eigen::MatrixBase< ConfigOut_t > &qout)


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:30