CODEWITHSUNDEEP
drake
FafaDog
FafaDog

Reputation: 188

How to move iiwa robot in Drake?

I'm trying to add a second iiwa robot to my simulation after figuring out how to add the first one. I'm currently attempting to move the first one using

iiwa_robot = plant.GetBodyByName("iiwa_link_0")
        X_B = plant.EvalBodyPoseInWorld(plant_context, iiwa_robot)

        
        tf = RigidTransform(p=[15, 10, 5]) 
plant.SetFreeBodyPose(plant_context,iiwa_robot,X_B.multiply(tf))

However I keep getting the error

Body 'iiwa_link_0' is not a free floating body

I've also tried the other iiwa_links_ which produced the same error. Am I supposed to change something in the sdf file? Is there a better way to do this that I'm just not thinking of?

Upvotes: 0

Views: 151

Answers (1)

Hongkai Dai
Hongkai Dai

Reputation: 2766

When you add your first IIWA arm, you can call WeldFrames to put the robot at the desired location.

Here is the pseudo code

iiwa1 = Parser(plant).AddModelFromFile(iiwa_file, "iiwa1")
# Weld the link0 of iiwa1 with the world at a given location.
plant.WeldFrames(plant.world_frame(), plant.GetBodyByName("iiwa_link_0", iiwa1).body_frame(), RigidTransform(p=[15, 10, 5]))

You can similarly add the second IIWA arm and weld its link_0 to the world to a different location.

iiwa2 = Parser(plant).AddModelFromFile(iiwa_file, "iiwa2")
# Weld the link0 of iiwa2 with the world at a given location.
plant.WeldFrames(plant.world_frame(), plant.GetBodyByName("iiwa_link_0", iiwa2).body_frame(), RigidTransform(p=[10, 10, 5]))

Upvotes: 1

Related Questions