Skip to content

Some questions about calculateJacobian() #3313

@zoukj

Description

@zoukj

Input

  1. 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?
  2. objVelocities: I thought Jacobian is a function of joint position. Why we need velocity to compute Jacobian?
  3. 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

Metadata

Metadata

Assignees

No one assigned

    Labels

    No labels
    No labels

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions