How to use calculateInverseKineMatics and setJointMotorControlArray to move robot end effector to coordinates
stdcout42 opened this issue · comments
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!
This problem has been solved by using setAngles
instead of PyBullet's setJointMotorControlArray
Glad you could solve it!
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?
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
.