How to use calculateInverseKineMatics and setJointMotorControlArray to move robot end effector to coordinates

[Saman Shahbazi]

Very new at this, I'm sure I must be doing something extremely naive. What I basically did was, I used the example file (robot_joint_controls) and extracted a position for the r_wrist - then I tried to have the bot move the right wrist to that extracted position in my script. However, it's not behaving as I was expecting.

I have the following, the move(...) method is being called in a while loop.

class PepperSimulator(object):
  def __init__(self):
    self.simulation_manager = SimulationManager()
    self.client = self.simulation_manager.launchSimulation(gui=True)
    self.pepper = self.simulation_manager.spawnPepper(self.client, spawn_ground_plane=True)
    self.bodyUniqueId = self.pepper.getRobotModel()
    print(f'initial position: {self.pepper.getLinkPosition("r_wrist")[0]}')

  def move(self, coords):
   coords = [-0.1268, -0.1663, 1.2294]
   endEffectorLinkIndex = self.pepper.link_dict["r_wrist"].getIndex()
   #p.stepSimulation()
   self.simulation_manager.stepSimulation(self.client)
   ik = p.calculateInverseKinematics(self.bodyUniqueId, endEffectorLinkIndex, coords)[:endEffectorLinkIndex-1]
   p.setJointMotorControlArray(self.bodyUniqueId, list(range(len(ik))), p.POSITION_CONTROL, targetPositions=ik)
   print(f'translation: {self.pepper.getLinkPosition("r_wrist")[0]}')

I have made the coords constant to be able to test out the workings, however, the link position never gets to that coordinate (it ends up around (0.2240585833787918, -0.16686952114105225, 0.8978853225708008) ).

Any help would be greatly appreciated!

[Saman Shahbazi]

This problem has been solved by using setAngles instead of PyBullet's setJointMotorControlArray

[Maxime Busy]

Glad you could solve it!

[Amit Ghimire]

Hi Can anyone point me how to use the setAngles method to move the arms ? I am not sure I understand this line "The kinematic chains start at the Tibia link and respectively end at the right and left gripper links."

How do I use the value returned by calculateInverseKinematics to set joint angles? Can anyone explain what the values returned by calculateInverseKinematics represent for pepper robot?

[Saman Shahbazi]

Check out the pybullet manual and search for InverseKinematics.

"You can compute the joint angles that makes the end-effector reach a given target position in
Cartesian world space."

Check out this section where I use IK to compute the joint angles and subsequently call setAngles.