Ros-Noetic: Python 3 - PyKDL Error
Hello I am trying to read a urdf file but I get error as
python3: /build/orocos-kdl-mPkyII/orocos-kdl-1.4.0/python_orocos_kdl/PyKDL/std_string.sip:52: int convertTo_std_string(PyObject*, void**, int*, PyObject*): Assertion `PyUnicode_Check(s)' failed.
I am using the recommended Ubuntu 20.04 (Focal Fossa) with ROS Noetic
It never used to happen on melodic with python2.7. I am not sure whether its a problem with the package urdfparserpy or is it with PyKDL?.
I get the error when i try to execute,
from __future__ import print_function
import PyKDL as kdl
import urdf_parser_py.urdf as urdf
with open(filename) as urdfFile:
return treeFromUrdfModel(urdf.URDF.from_xml_string(urdfFile.read()))
I think my problem is similar to this, https://github.com/orocos/orocos_kinematics_dynamics/issues/115#issuecomment-388792903
and I think this is the solution, https://github.com/orocos/orocos_kinematics_dynamics/pull/145#issue-187920168
But I am not sure how should I edit and incorporate the changes mentioned above.
My urdf file is,
<?xml version="1.0"?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from ur5_joint_limited_robot.urdf.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="ur5">
<gazebo>
<plugin filename="libgazebo_ros_control.so" name="ros_control">
<!-- <robotNamespace>/</robotNamespace> -->
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
<legacyModeNS>true</legacyModeNS>
</plugin>
<!--
<plugin name="gazebo_ros_power_monitor_controller" filename="libgazebo_ros_power_monitor.so">
<alwaysOn>true</alwaysOn>
<updateRate>1.0</updateRate>
<timeout>5</timeout>
<powerStateTopic>power_state</powerStateTopic>
<powerStateRate>10.0</powerStateRate>
<fullChargeCapacity>87.78</fullChargeCapacity>
<dischargeRate>-474</dischargeRate>
<chargeRate>525</chargeRate>
<dischargeVoltage>15.52</dischargeVoltage>
<chargeVoltage>16.41</chargeVoltage>
</plugin>
-->
</gazebo>
<!-- camera_link -->
<gazebo reference="camera_link">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/Green</material>
</gazebo>
<!-- camera -->
<gazebo reference="camera_link">
<sensor name="camera1" type="camera">
<update_rate>30.0</update_rate>
<camera name="head">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>800</width>
<height>800</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame.
That pixel's noise value is added to each of its color
channels, which at that point lie in the range [0,1]. -->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
<plugin filename="libgazebo_ros_camera.so" name="camera_controller">
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<cameraName>ur5/usbcam</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>camera_link_optical</frameName>
<!-- setting hackBaseline to anything but 0.0 will cause a misalignment
between the gazebo sensor image and the frame it is supposed to
be attached to -->
<hackBaseline>0.0</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
<CxPrime>0</CxPrime>
<Cx>0.0</Cx>
<Cy>0.0</Cy>
<focalLength>0.0</focalLength>
</plugin>
</sensor>
</gazebo>
<!-- measured from model -->
<!--property name="shoulder_height" value="0.089159" /-->
<!--property name="shoulder_offset" value="0.13585" /-->
<!-- shoulder_offset - elbow_offset + wrist_1_length = 0.10915 -->
<!--property name="upper_arm_length" value="0.42500" /-->
<!--property name="elbow_offset" value="0.1197" /-->
<!-- CAD measured -->
<!--property name="forearm_length" value="0.39225" /-->
<!--property name="wrist_1_length" value="0.093" /-->
<!-- CAD measured -->
<!--property name="wrist_2_length" value="0.09465" /-->
<!-- In CAD this distance is 0.930, but in the spec it is 0.09465 -->
<!--property name="wrist_3_length" value="0.0823" /-->
<!-- manually measured -->
<link name="base_link">
<visual>
<geometry>
<mesh filename="package://ur_description/meshes/ur5/visual/base.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
<mesh filename="package://ur_description/meshes/ur5/collision/base.stl"/>
</geometry>
</collision>
<inertial>
<mass value="4.0"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<inertia ixx="0.00443333156" ixy="0.0" ixz="0.0" iyy="0.00443333156" iyz="0.0" izz="0.0072"/>
</inertial>
</link>
<joint name="shoulder_pan_joint" type="revolute">
<parent link="base_link"/>
<child link="shoulder_link"/>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.089159"/>
<axis xyz="0 0 1"/>
<limit effort="150.0" lower="-3.14159265359" upper="3.14159265359" velocity="3.15"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
<link name="shoulder_link">
<visual>
<geometry>
<mesh filename="package://ur_description/meshes/ur5/visual/shoulder.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
<mesh filename="package://ur_description/meshes/ur5/collision/shoulder.stl"/>
</geometry>
</collision>
<inertial>
<mass value="3.7"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<inertia ixx="0.010267495893" ixy="0.0" ixz="0.0" iyy="0.010267495893" iyz="0.0" izz="0.00666"/>
</inertial>
</link>
<joint name="shoulder_lift_joint" type="revolute">
<parent link="shoulder_link"/>
<child link="upper_arm_link"/>
<origin rpy="0.0 1.57079632679 0.0" xyz="0.0 0.13585 0.0"/>
<axis xyz="0 1 0"/>
<limit effort="150.0" lower="-3.14159265359" upper="3.14159265359" velocity="3.15"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
<link name="upper_arm_link">
<visual>
<geometry>
<mesh filename="package://ur_description/meshes/ur5/visual/upperarm.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
<mesh filename="package://ur_description/meshes/ur5/collision/upperarm.stl"/>
</geometry>
</collision>
<inertial>
<mass value="8.393"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.28"/>
<inertia ixx="0.22689067591" ixy="0.0" ixz="0.0" iyy="0.22689067591" iyz="0.0" izz="0.0151074"/>
</inertial>
</link>
<joint name="elbow_joint" type="revolute">
<parent link="upper_arm_link"/>
<child link="forearm_link"/>
<origin rpy="0.0 0.0 0.0" xyz="0.0 -0.1197 0.425"/>
<axis xyz="0 1 0"/>
<limit effort="150.0" lower="-3.14159265359" upper="3.14159265359" velocity="3.15"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
<link name="forearm_link">
<visual>
<geometry>
<mesh filename="package://ur_description/meshes/ur5/visual/forearm.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
<mesh filename="package://ur_description/meshes/ur5/collision/forearm.stl"/>
</geometry>
</collision>
<inertial>
<mass value="2.275"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.196125"/>
<inertia ixx="0.0312167910289" ixy="0.0" ixz="0.0" iyy="0.0312167910289" iyz="0.0" izz="0.004095"/>
</inertial>
</link>
<joint name="wrist_1_joint" type="revolute">
<parent link="forearm_link"/>
<child link="wrist_1_link"/>
<origin rpy="0.0 1.57079632679 0.0" xyz="0.0 0.0 0.39225"/>
<axis xyz="0 1 0"/>
<limit effort="28.0" lower="-3.14159265359" upper="3.14159265359" velocity="3.2"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
<link name="wrist_1_link">
<visual>
<geometry>
<mesh filename="package://ur_description/meshes/ur5/visual/wrist1.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
<mesh filename="package://ur_description/meshes/ur5/collision/wrist1.stl"/>
</geometry>
</collision>
<inertial>
<mass value="1.219"/>
<origin rpy="0 0 0" xyz="0.0 0.093 0.0"/>
<inertia ixx="0.00255989897604" ixy="0.0" ixz="0.0" iyy="0.00255989897604" iyz="0.0" izz="0.0021942"/>
</inertial>
</link>
<joint name="wrist_2_joint" type="revolute">
<parent link="wrist_1_link"/>
<child link="wrist_2_link"/>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.093 0.0"/>
<axis xyz="0 0 1"/>
<limit effort="28.0" lower="-3.14159265359" upper="3.14159265359" velocity="3.2"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
<link name="wrist_2_link">
<visual>
<geometry>
<mesh filename="package://ur_description/meshes/ur5/visual/wrist2.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
<mesh filename="package://ur_description/meshes/ur5/collision/wrist2.stl"/>
</geometry>
</collision>
<inertial>
<mass value="1.219"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.09465"/>
<inertia ixx="0.00255989897604" ixy="0.0" ixz="0.0" iyy="0.00255989897604" iyz="0.0" izz="0.0021942"/>
</inertial>
</link>
<joint name="wrist_3_joint" type="revolute">
<parent link="wrist_2_link"/>
<child link="wrist_3_link"/>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.09465"/>
<axis xyz="0 1 0"/>
<limit effort="28.0" lower="-3.14159265359" upper="3.14159265359" velocity="3.2"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
<link name="wrist_3_link">
<visual>
<geometry>
<mesh filename="package://ur_description/meshes/ur5/visual/wrist3.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
<mesh filename="package://ur_description/meshes/ur5/collision/wrist3.stl"/>
</geometry>
</collision>
<inertial>
<mass value="0.1879"/>
<origin rpy="1.57079632679 0 0" xyz="0.0 0.06505 0.0"/>
<inertia ixx="8.46958911216e-05" ixy="0.0" ixz="0.0" iyy="8.46958911216e-05" iyz="0.0" izz="0.0001321171875"/>
</inertial>
</link>
<joint name="ee_fixed_joint" type="fixed">
<parent link="wrist_3_link"/>
<child link="ee_link"/>
<origin rpy="0.0 0.0 1.57079632679" xyz="0.0 0.0823 0.0"/>
</joint>
<link name="ee_link">
<collision>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
<origin rpy="0 0 0" xyz="-0.01 0 0"/>
</collision>
</link>
<joint name="camera_joint" type="fixed">
<axis xyz="0 1 0"/>
<origin rpy="0 0 0" xyz="-0.02 0.0 0.055"/>
<parent link="ee_link"/>
<child link="camera_link"/>
</joint>
<!-- Camera -->
<link name="camera_link">
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.02 0.02 0.02"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.02 0.02 0.02"/>
</geometry>
<material name="green"/>
</visual>
<inertial>
<mass value="1e-5"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
</inertial>
</link>
<!-- generate an optical frame http://www.ros.org/reps/rep-0103.html#suffix-frames
so that ros and opencv can operate on the camera frame correctly -->
<joint name="camera_optical_joint" type="fixed">
<!-- these values have to be these values otherwise the gazebo camera image
won't be aligned properly with the frame it is supposedly originating from -->
<origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 0 0"/>
<parent link="camera_link"/>
<child link="camera_link_optical"/>
</joint>
<link name="camera_link_optical">
</link>
<transmission name="shoulder_pan_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="shoulder_pan_joint">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="shoulder_pan_motor">
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="shoulder_lift_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="shoulder_lift_joint">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="shoulder_lift_motor">
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="elbow_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="elbow_joint">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="elbow_motor">
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="wrist_1_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="wrist_1_joint">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="wrist_1_motor">
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="wrist_2_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="wrist_2_joint">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="wrist_2_motor">
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="wrist_3_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="wrist_3_joint">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="wrist_3_motor">
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<gazebo reference="shoulder_link">
<selfCollide>true</selfCollide>
<implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
<gazebo reference="upper_arm_link">
<selfCollide>true</selfCollide>
<implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
<gazebo reference="forearm_link">
<selfCollide>true</selfCollide>
<implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
<gazebo reference="wrist_1_link">
<selfCollide>true</selfCollide>
<implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
<gazebo reference="wrist_3_link">
<selfCollide>true</selfCollide>
<implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
<gazebo reference="wrist_2_link">
<selfCollide>true</selfCollide>
<implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
<gazebo reference="ee_link">
<selfCollide>true</selfCollide>
<implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
<!-- ROS base_link to UR 'Base' Coordinates transform -->
<link name="base"/>
<joint name="base_link-base_fixed_joint" type="fixed">
<!-- NOTE: this rotation is only needed as long as base_link itself is
not corrected wrt the real robot (ie: rotated over 180
degrees)
-->
<origin rpy="0 0 -3.14159265359" xyz="0 0 0"/>
<parent link="base_link"/>
<child link="base"/>
</joint>
<!-- Frame coincident with all-zeros TCP on UR controller -->
<link name="tool0"/>
<joint name="wrist_3_link-tool0_fixed_joint" type="fixed">
<origin rpy="-1.57079632679 0 0" xyz="0 0.0823 0"/>
<parent link="wrist_3_link"/>
<child link="tool0"/>
</joint>
<link name="world"/>
<joint name="world_joint" type="fixed">
<parent link="world"/>
<child link="base_link"/>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.72 0.0"/>
</joint>
<link name="ee_ball">
<visual>
<geometry>
<sphere radius="0.015"/>
</geometry>
</visual>
</link>
<joint name="ee_ball_joint" type="fixed">
<origin rpy="0 0 0" xyz="0.16 0.0 0.0"/>
<parent link="ee_link"/>
<child link="ee_ball"/>
</joint>
<gazebo reference="ee_ball">
<transparency>0.9</transparency>
<material>Gazebo/Red</material>
</gazebo>
<link name="ee_dummy">
<visual>
<geometry>
<sphere radius="0.015"/>
</geometry>
</visual>
</link>
<joint name="ee_dummy_joint" type="fixed">
<origin xyz="0.16 0.0 0.0" rpy="0 0 0" />
<parent link="ee_ball"/>
<child link="ee_dummy"/>
</joint>
</robot>
Asked by mkb_10062949 on 2020-05-24 08:21:08 UTC
Comments