Search code examples
pythontransformationroboticsdrake

Getting joint location(3D pose) w.r.t world of Kuka Iiwa arm in Drake

I have a Kuka Iiwa 7 arm in the scene and I am interested in getting the current(real-time) 3D coordinates of the arm joints w.r.t world (or w.r.t robot base). I am able to get the current joint 1 DOF positions(thetas) but not the exact 3D coordinate representing the position of the joint in some frame(world or base).

I tried adding Triad for each link and tried get_pose_in_world() method for RigidBody_[float] type object of the frame, but received an error that the method does not exist for RigidBody_[float] class.

Note: I am using PyDrake for the project.

What is the correct way to approach this problem?

Solution

  • When the parser creates a joint, it'll also crate a "parent" frame and a "child" frame. These two frames are then constrained by the joint such that in the "zero configuration" (say zero angle) the parent and child frames are coincident.

    If you do happen to have the frame object, then you can call Frame::CalcPoseInWorld(). Now, to retrieve the frame, you need to know its name. For that you will call GetFrameByName().

    Say you want the pose of the parent frame. Here is the convention used to name frames:

    • If the pose of the parent frame in the body frame is the identity (either a default or explicitly provided) then the parent frame IS the body frame and you can retrieve it with the body's name.
    • If the pose is not the identity, then a new frame is created (offset by that pose) and its name is "parent_[body_name]", where body_name is the name of the parent body.