-
Notifications
You must be signed in to change notification settings - Fork 3.1k
Closed
Description
Input
- objPosition: This variable don't include the base position and orientation. So the Jacobian can only be used to compute velocity relative to the base, right?
- objVelocities: I thought Jacobian is a function of joint position. Why we need velocity to compute Jacobian?
- objAcceleration: What's the use of desired joint accelerations for Jacobian?
Example
If I use Jacobian to compute angular velocity of a link, it will be equal to the result of getLinkState().
(for a rigid body, angular velocity should be equal everywhere on it)
but I found they are not the same, what's wrong with my code?
import time
# TODO: for example, about the last link and joint
index = p.getNumJoints(model_id)-1
while True:
base_lin_vel, base_ang_vel = np.array(p.getBaseVelocity(model_id))
positions, velosities = [], []
for i in range(p.getNumJoints(model_id)):
pos, vel = p.getJointState(model_id, i)[0:2]
positions.append(pos)
velosities.append(vel)
pass
q_dot = np.concatenate([base_ang_vel,
base_lin_vel,
velosities])
lin_J, ang_J = p.calculateJacobian(model_id,
linkIndex=index,
localPosition=[0, 0, 0],
objPositions=positions,
objVelocities=velosities,
objAccelerations=len(positions)*[0.])
lin_J, ang_J = np.array(lin_J), np.array(ang_J)
base_orn = p.getBasePositionAndOrientation(model_id)[1]
# only consider the angular velocity
vel = ang_J @ q_dot
vel = p.rotateVector(base_orn, vel)
print(vel)
actual = p.getLinkState(model_id, index,
computeLinkVelocity=1,
computeForwardKinematics=1)[7]
print(actual)
print('~'*50)
p.stepSimulation()
time.sleep(1./240)
pass
Reactions are currently unavailable
Metadata
Metadata
Assignees
Labels
No labels