Update link parameters during simulation

[akhilsathuluri]

I am trying to simulate multiple robots with variations in link lengths and radii which in turn effect their mass and inertia. I can use the changeDynamics method to vary the mass and other dynamic properties. Is there a way to modify the link lengths of the robot online without manually changing the URDF file?

[erwincoumans]

No dynamic changes for link length or joint frame etc. you could use the urdf editor to load a urdf, change stuff and programmatic create an instance. see in pybullet_utils

[akhilsathuluri]

Thanks for the answer! I have added an example of doing this as a reply.

[jgoenetxea]

You can generate a new version of the reference robot using a python function and a xml packages. It is a simple trick but it works.

The main idea is to load the xml file (URDF files are based on xml) and generate an additional file in a temporal location with the new size values.

import os
import xml.etree.ElementTree as ET

def createSizedURDFFile(oriModelFile, robot_sizes):
    def update_link_size(tag, inertia_pos, length, s=1):
        data_str = '{} {} {}'.format((inertia_pos[0] / 2) * s,
                                        (inertia_pos[1] / 2) * s,
                                        (inertia_pos[2] / 2) * s)
        tag.find('inertial').find('origin').set('xyz', data_str)
        tag.find('collision').find('origin').set('xyz', data_str)
        if not length is None:
            length_str = '{}'.format(length * s)
            # tag.find('collision').find('geometry').find('capsule').set('length', length_str)
            tag.find('collision').find('geometry').find(
                'cylinder').set('length', length_str)

    def update_joint_size(tag, joint_location, s=1):
        data_str = '{} {} {}'.format(joint_location[0] * s,
                                     joint_location[1] * s,
                                     joint_location[2] * s)
        tag.find('origin').set('xyz', data_str)

    # Generate output file name
    file_name_parts = os.path.splitext(oriModelFile)
    outModelFile = file_name_parts[0] + '_new' + file_name_parts[1]
    # Define unit transform scale
    data_scale = 0.01
    # Read the entire xml file
    body_data_tree = ET.parse(oriModelFile)
    #   - get the parent tag
    root_tag = body_data_tree.getroot()
    # Replace the desired values
    inertia_pos = []
    joint_location = []
    for link in root_tag.findall('link'):
        name = link.get('name')
        update = False
        length = None
        if name == 'left_leg':
            inertia_pos = [0, robot_sizes.left_leg_length, 0]
            update = True
        elif name == 'right_leg':
            inertia_pos = [0, robot_sizes.right_leg_length, 0]
            update = True
        # Update values
        if update:
            update_link_size(link, inertia_pos, length, data_scale)

    for joint in root_tag.findall('joint'):
        name = joint.get('name')
        update = False
        if name == 'left_knee':
            joint_location = [0, robot_sizes.left_leg_length, 0]
            update = True
        elif name == 'right_knee':
            joint_location = [0, robot_sizes.right_leg_length, 0]
            update = True
        # Update values
        if update:
            update_joint_size(joint, joint_location, data_scale)
    # Load the body model from the generated string
    # data_string = ET.tostring(root_tag, encoding='unicode', method='xml')
    with open(outModelFile, 'wb') as f:
        body_data_tree.write(f, encoding='utf-8')

    return outModelFile

[akhilsathuluri]

Thank you for the suggestion.

I did explore the possibility of modifying the XML file to programmatically update the link lengths and generate new URDFs files. See also odio_urdf for directly generating URDFs in python.

However, the modified file, outModelFile, still needs to be saved to the disk as a URDF file before loading it to the pybullet server. Is there a way to directly upload the updated model to the pybullets server without creating an intermediate .URDF file?

I was hoping to see if there is a way to modify the URDF online after being uploaded to the server. On the other hand, is there a way to directly load the outModelFile to pybullet?; as the loadURDF command requires a file location and not the XML file itself.

Thanks for the help!

[erwincoumans]

You can use the pybullet_utils.urdfEditor.py for this, or programmatically create the multibody.
See https://github.com/bulletphysics/bullet3/blob/5ae9a15ecac7bc7e71f1ec1b544a55135d7d7e32/examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py

The UrdfEditor is a starting point, some parameters are probably not handled (left as an exercise for the user)

[akhilsathuluri]

An example of how the link lengths could be changed online for a simple cylindrical link:

<?xml version="1.0"?>
<robot  name="simple_cylinder" >
 <material  name="red" >
  <color  rgba="0.8 0 0 1.0" />
 </material>
 <link  name="base_link" >
   <inertial >
    <origin  xyz="0 0 0.058"  rpy="0 0 0" />
    <mass  value="1.1734476879688598" />
    <inertia  ixy="0"  ixz="0"  iyz="0"  izz="0.00023468953759377196"  iyy="0.00526330402976966"  ixx="0.00526330402976966" />
   </inertial>
   <visual >
     <origin  xyz="0 0 0"  rpy="0 0 0" />
     <material  name="red" />
     <geometry >
      <cylinder  radius="0.02"  length="0.116" />
     </geometry>
   </visual>
   <!-- <collision >
    <origin  xyz="0 0 0.058"  rpy="0 0 0" />
    <geometry >
     <cylinder  radius="0.02"  length="0.116" />
    </geometry>
   </collision> -->
 </link>
</robot>

Using the urdfEditor from pubullet_utils:

from pybullet_utils import urdfEditor as ed
import pybullet
import pybullet_data

p0 = bc.BulletClient(connection_mode=pybullet.GUI)
p0.setAdditionalSearchPath(pybullet_data.getDataPath())

body_id = p0.loadURDF("simple_cylinder.urdf")
ed0 = ed.UrdfEditor()
ed0.initializeFromBulletBody(body_id, p0._client)

l = ed0.urdfLinks
l[0].urdf_visual_shapes[0].geom_length = 0.5
ed0.createMultiBody([1, 0, 0])

However I could not make it work with the collision tag in the urdf file. urdfEditor seems to recognise the collision shape p.GEOM_CYLINDER as p.GEOM_MESH and is searching for the mesh files. Also, as a note, for a general serial robot, the link size can be changed using the above code snippet, however, the entire kinematic structure should be changed along with it (the current and the next joints' origins should be changed accordingly).
Hope it helps.