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
def compute_effector_position(self, msg):
positions = JntArray(3)
chain = Chain()
kdl_frame = Frame()
d = 0
theta = 0
order = [1, 2, 0]
for i in order:
joint = self.params[i]
a = joint["a"]
d = joint["d"]
alpha = joint["alpha"]
theta = joint["theta"]
frame = kdl_frame.DH(a, alpha, d, theta)
chain.addSegment(Segment(Joint(Joint.RotZ), frame))
positions[i] = msg.position[i]
fk_solver = ChainFkSolverPos_recursive(chain)
result = Frame()
fk_solver.JntToCart(positions, result)
kdl_pose = PoseStamped()
kdl_pose.header.frame_id = 'base_link'
kdl_pose.header.stamp = rospy.Time.now()
kdl_pose.pose.position.x = result.p[0]
kdl_pose.pose.position.y = result.p[1]
kdl_pose.pose.position.z = result.p[2]
quat = result.M.GetQuaternion()
kdl_pose.pose.orientation.x = quat[0]
kdl_pose.pose.orientation.y = quat[1]
kdl_pose.pose.orientation.z = quat[2]
kdl_pose.pose.orientation.w = quat[3]
return kdl_pose
