Commit f2ea4d2f authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

[examples] Improve and test overview-urdf.py

parent 6fa1c7d5
......@@ -40,7 +40,7 @@ IF(BUILD_PYTHON_INTERFACE)
SET(${PROJECT_NAME}_PYTHON_EXAMPLES
i-inverse-kinematics
overview-simple
#overview-urdf
overview-urdf
)
FOREACH(EXAMPLE ${${PROJECT_NAME}_PYTHON_EXAMPLES})
......
......@@ -33,8 +33,10 @@ int main(int argc, char ** argv)
forwardKinematics(model,data,q);
// Print out the placement of each joint of the kinematic tree
for(JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id)
std::cout << model.names[joint_id] << "\t\t: "
for(JointIndex joint_id = 0; joint_id < (JointIndex)model.njoints; ++joint_id)
std::cout << std::setw(24) << std::left
<< model.names[joint_id] << ": "
<< std::fixed << std::setprecision(2)
<< data.oMi[joint_id].translation().transpose()
<< std::endl;
}
import pinocchio
from sys import argv
from os.path import dirname, join, abspath
filename = "ur5.urdf" if len(argv)<2 else argv[1]
model = pinocchio.buildModelFromUrdf(filename)
# This path refers to Pinocchio source code but you can define your own directory here.
pinocchio_model_dir = join(dirname(dirname(abspath(__file__))), 'models')
# You should change here to set up your own URDF file or just pass it as an argument of this example.
urdf_filename = pinocchio_model_dir + '/others/robots/ur_description/urdf/ur5_robot.urdf' if len(argv)<2 else argv[1]
# Load the urdf model
model = pinocchio.buildModelFromUrdf(urdf_filename)
# Create data required by the algorithms
data = model.createData()
# Sample a random configuration
q = pinocchio.randomConfiguration(model)
print('q = ', q.T)
print('q: %s' % q.T)
# Perform the forward kinematics over the kinematic tree
pinocchio.forwardKinematics(model,data,q)
for k in range(model.njoints):
# Print out the placement of each joint of the kinematic tree
for name, oMi in zip(model.names, data.oMi):
print(("{:<24} : {: .2f} {: .2f} {: .2f}"
.format( model.names[k], *data.oMi[k].translation.T.flat )))
.format( name, *oMi.translation.T.flat )))
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