def _get_jacobians(self, state):
"""Produce a Jacobian from the urdf that maps from joint angles to x, y, z.
This makes a 6x6 matrix from 6 joint angles to x, y, z and 3 angles.
The angles are roll, pitch, and yaw (not Euler angles) and are not needed.
Returns a repackaged Jacobian that is 3x6.
"""
# Initialize a Jacobian for 6 joint angles by 3 cartesian coords and 3 orientation angles
jacobian = Jacobian(6)
# Initialize a joint array for the present 6 joint angles.
angles = JntArray(6)
# Construct the joint array from the most recent joint angles.
for i in range(6):
angles[i] = state[i]
# Initialize a tree structure from the robot urdf.
# Note that the xacro of the urdf is updated by hand.
# Then the urdf must be compiled.
_, ur_tree = treeFromFile(self._hyperparams['tree_path'])
# Retrieve a chain structure between the base and the start of the end effector.
ur_chain = ur_tree.getChain(self._hyperparams['link_names'][0],
self._hyperparams['link_names'][6])
# Initialize a KDL Jacobian solver from the chain.
jac_solver = ChainJntToJacSolver(ur_chain)
# Update the jacobian by solving for the given angles.
jac_solver.JntToJac(angles, jacobian)
# Initialize a numpy array to store the Jacobian.
J = np.zeros((6, 6))
for i in range(jacobian.rows()):
for j in range(jacobian.columns()):
J[i, j] = jacobian[i, j]
# Only want the cartesian position, not Roll, Pitch, Yaw (RPY) Angles
ee_jacobians = J[0:3, :]
return ee_jacobians
def get_jacobians(self, state, robot_id=0):
"""
Produce a Jacobian from the urdf that maps from joint angles to x, y, z.
This makes a 6x6 matrix from 6 joint angles to x, y, z and 3 angles.
The angles are roll, pitch, and yaw (not Euler angles) and are not needed.
Returns a repackaged Jacobian that is 3x6.
"""
# Initialize a Jacobian for self.scara_chain.getNrOfJoints() joint angles by 3 cartesian coords and 3 orientation angles
jacobian = Jacobian(self.scara_chain.getNrOfJoints())
# Initialize a joint array for the present self.scara_chain.getNrOfJoints() joint angles.
angles = JntArray(self.scara_chain.getNrOfJoints())
# Construct the joint array from the most recent joint angles.
for i in range(self.scara_chain.getNrOfJoints()):
angles[i] = state[i]
# Update the jacobian by solving for the given angles.observation_callback
self.jac_solver.JntToJac(angles, jacobian)
# Initialize a numpy array to store the Jacobian.
J = np.array([[jacobian[i, j] for j in range(jacobian.columns())] for i in range(jacobian.rows())])
# Only want the cartesian position, not Roll, Pitch, Yaw (RPY) Angles
ee_jacobians = J
return ee_jacobians
