How to set the transform of the root link of an element added to the world?

[clemense]

Hi, I'm trying to set the transformation of the root link of a kinematic chain after loading it into the scene. No success so far. Last thing I tried:

coordinates.setWorldModel(world)
coordinates.setFrameCoordinates(name='panda:root', coordinates=klampt.math.se3.from_translation([0, 0, 3]))
coordinates.updateToWorld()

What's the recommended way to achieve this?

Thanks!

[krishauser]

The coordinates module doesn't actually alter the position of the robot model; it's just a collection of transforms that is useful for doing coordinate conversion.

Instead, you should modify the first link's parent transform in the robot model:

robot = world.robot(0) robot.link(0).setParentTransform(klampt.math.se3.from_translation([0,0,3]))

[clemense]

Thanks!

I think my main confusion stems from the visualizer and the coordinate manager not showing this offset. If I do the following in an IPython console:

import klampt
from klampt.model import coordinates
from klampt import *
world = WorldModel()
robot = world.loadRobot('/data/robot-assets/urdfs/robots/franka_panda/panda.urdf')
robot.link(0).setParentTransform(R=klampt.math.so3.identity(), t=[0,0,3.0])
coordinates.setWorldModel(world)
coordinates.updateFromWorld()
vis.init('PyQt')
vis.add("world", world)
vis.add('world_frame', coordinates.Frame('world'))
vis.add('panda_link0', coordinates.Frame('panda:panda_link0'))
vis.show()
print(coordinates.Frame('panda:panda_link0').worldCoordinates())

The viz shows both frames on top of each other and the output of the last command is:

Out[15]: ([1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], [0.0, 0.0, 0.0])

Is it because I'm using the coordinate manager in a wrong way?

[krishauser]

Oh sorry, it’s because forward kinematics is not updated when setParentTransform is called. Try calling robot.setConfig(robot.getConfig()) right afterwards