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 = (
9  join(pinocchio_model_dir, "example-robot-data/robots") if len(argv) < 2 else argv[1]
10 )
11 mesh_dir = pinocchio_model_dir
12 urdf_model_path = join(model_path, "ur_description/urdf/ur5_robot.urdf")
13 
14 # Load the urdf model
15 model, collision_model, visual_model = pinocchio.buildModelsFromUrdf(
16  urdf_model_path, mesh_dir
17 )
18 print("model name: " + model.name)
19 
20 # Create data required by the algorithms
21 data, collision_data, visual_data = pinocchio.createDatas(
22  model, collision_model, visual_model
23 )
24 
25 # Sample a random configuration
27 print("q: %s" % q.T)
28 
29 # Perform the forward kinematics over the kinematic tree
30 pinocchio.forwardKinematics(model, data, q)
31 
32 # Update Geometry models
33 pinocchio.updateGeometryPlacements(model, data, collision_model, collision_data)
34 pinocchio.updateGeometryPlacements(model, data, visual_model, visual_data)
35 
36 # Print out the placement of each joint of the kinematic tree
37 print("\nJoint placements:")
38 for name, oMi in zip(model.names, data.oMi):
39  print(("{:<24} : {: .2f} {: .2f} {: .2f}".format(name, *oMi.translation.T.flat)))
40 
41 # Print out the placement of each collision geometry object
42 print("\nCollision object placements:")
43 for k, oMg in enumerate(collision_data.oMg):
44  print(("{:d} : {: .2f} {: .2f} {: .2f}".format(k, *oMg.translation.T.flat)))
45 
46 # Print out the placement of each visual geometry object
47 print("\nVisual object placements:")
48 for k, oMg in enumerate(visual_data.oMg):
49  print(("{:d} : {: .2f} {: .2f} {: .2f}".format(k, *oMg.translation.T.flat)))
pinocchio::updateGeometryPlacements
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...
pinocchio::randomConfiguration
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::forwardKinematics
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...
path


pinocchio
Author(s):
autogenerated on Sun Jun 16 2024 02:43:10