|
| 1 | +#!/usr/bin/env python |
| 2 | + |
| 3 | +import numpy |
| 4 | + |
| 5 | +import geometry_msgs.msg |
| 6 | +import rospy |
| 7 | +from sensor_msgs.msg import JointState |
| 8 | +import tf |
| 9 | +import tf.msg |
| 10 | +from urdf_parser_py.urdf import URDF |
| 11 | + |
| 12 | +""" Starting from a computed transform T, creates a message that can be |
| 13 | +communicated over the ROS wire. In addition to the transform itself, the message |
| 14 | +also specifies who is related by this transform, the parent and the child.""" |
| 15 | +def convert_to_message(T, child, parent): |
| 16 | + t = geometry_msgs.msg.TransformStamped() |
| 17 | + t.header.frame_id = parent |
| 18 | + t.header.stamp = rospy.Time.now() |
| 19 | + t.child_frame_id = child |
| 20 | + translation = tf.transformations.translation_from_matrix(T) |
| 21 | + rotation = tf.transformations.quaternion_from_matrix(T) |
| 22 | + t.transform.translation.x = translation[0] |
| 23 | + t.transform.translation.y = translation[1] |
| 24 | + t.transform.translation.z = translation[2] |
| 25 | + t.transform.rotation.x = rotation[0] |
| 26 | + t.transform.rotation.y = rotation[1] |
| 27 | + t.transform.rotation.z = rotation[2] |
| 28 | + t.transform.rotation.w = rotation[3] |
| 29 | + return t |
| 30 | + |
| 31 | +#Our main class for computing Forward Kinematics |
| 32 | +class ForwardKinematics(object): |
| 33 | + |
| 34 | + |
| 35 | + #Initialization |
| 36 | + def __init__(self): |
| 37 | + """Announces that it will publish forward kinematics results, in the form of tfMessages. |
| 38 | + "tf" stands for "transform library", it's ROS's way of communicating around information |
| 39 | + about where things are in the world""" |
| 40 | + self.pub_tf = rospy.Publisher("/tf", tf.msg.tfMessage, queue_size=1) |
| 41 | + |
| 42 | + #Loads the robot model, which contains the robot's kinematics information |
| 43 | + self.robot = URDF.from_parameter_server() |
| 44 | + |
| 45 | + #Subscribes to information about what the current joint values are. |
| 46 | + rospy.Subscriber("joint_states", JointState, self.callback) |
| 47 | + |
| 48 | + |
| 49 | + """This function is called every time the robot publishes its joint values. We must use |
| 50 | + the information we get to compute forward kinematics. |
| 51 | +
|
| 52 | + We will iterate through the entire chain, and publish the transform for each link we find. |
| 53 | + """ |
| 54 | + def callback(self, unordered_joint_values): |
| 55 | + # We will start at the root |
| 56 | + link = self.robot.get_root() |
| 57 | + # This will hold our results, which we will publish at the end |
| 58 | + all_transforms = tf.msg.tfMessage() |
| 59 | + # Prepare structures to hold information |
| 60 | + link_transforms = [] |
| 61 | + joints = [] |
| 62 | + joint_values = [] |
| 63 | + links = [] |
| 64 | + # Append the root with a fake fixed joint |
| 65 | + links.append(link) |
| 66 | + (jn, nl) = self.robot.child_map[link][0] |
| 67 | + root_joint = self.robot.joint_map[jn] |
| 68 | + root_joint.type='fixed' |
| 69 | + joints.append(root_joint) |
| 70 | + joint_values.append(0) |
| 71 | + # Cycle through the joints and collect relevant information |
| 72 | + while True: |
| 73 | + # Find the joint connected at the end of this link, or its "child" |
| 74 | + # Make sure this link has a child |
| 75 | + if link not in self.robot.child_map: |
| 76 | + break |
| 77 | + # Make sure it has a single child (we don't deal with forks) |
| 78 | + if len(self.robot.child_map[link]) != 1: |
| 79 | + rospy.logerror("Forked kinematic chain!") |
| 80 | + break |
| 81 | + # Get the name of the child joint, as well as the link it connects to |
| 82 | + (joint_name, next_link) = self.robot.child_map[link][0] |
| 83 | + # Get the actual joint based on its name |
| 84 | + if joint_name not in self.robot.joint_map: |
| 85 | + rospy.logerror("Joint not found in map") |
| 86 | + break; |
| 87 | + joint = self.robot.joint_map[joint_name] |
| 88 | + # Append link transform to list |
| 89 | + link_transforms.append(joint.origin) |
| 90 | + joints.append(joint) |
| 91 | + # Find the relevant joint axis and value and append to list |
| 92 | + if joint.type != 'fixed' and joint.name in unordered_joint_values.name: |
| 93 | + index = unordered_joint_values.name.index(joint.name) |
| 94 | + joint_values.append(unordered_joint_values.position[index]) |
| 95 | + else: |
| 96 | + joint_values.append(0) |
| 97 | + # Append the next link to list |
| 98 | + links.append(next_link) |
| 99 | + # Move to the next link |
| 100 | + link = next_link |
| 101 | + |
| 102 | + # Append one final identity link transform which is not used |
| 103 | + link_transforms.append(link_transforms[-1]) |
| 104 | + link_transforms[-1].rpy=(0,0,0) |
| 105 | + link_transforms[-1].xyz=(0,0,0) |
| 106 | + |
| 107 | + # Compute all transforms |
| 108 | + transforms = self.forward_kinematics(link_transforms, joints, joint_values) |
| 109 | + |
| 110 | + # Check if the right number if transforms has been returned |
| 111 | + if len(transforms) != len(links): |
| 112 | + print 'Transforms returned: ' + str(len(transforms)) |
| 113 | + rospy.logerror("Incorrect number of transforms returned") |
| 114 | + return |
| 115 | + |
| 116 | + # Convert the result into a message and prepare it to be published |
| 117 | + for i in range(0,len(transforms)): |
| 118 | + transform_msg = convert_to_message(transforms[i], links[i], "world_origin") |
| 119 | + all_transforms.transforms.append(transform_msg) |
| 120 | + |
| 121 | + # Publish all the transforms |
| 122 | + self.pub_tf.publish(all_transforms) |
| 123 | + |
| 124 | + |
| 125 | + """ This is the core function for our forward kinematics. Given information about all the links |
| 126 | + and joints of the robot, as well as the current joint angles, it must compute the transform from |
| 127 | + the base frame to each of the link coordinate frames of the robot. |
| 128 | +
|
| 129 | + Parameters are as follows: |
| 130 | +
|
| 131 | + - "link_transforms" is a vector containing the rigid transform associated with each link of the robot, |
| 132 | + as designed by the manufacturer. link_transforms[i] contains T_L_i, as described in the homework |
| 133 | + handout. The information is encoded as: |
| 134 | + * link_transforms[i].xyz: the translation component of T_L_i |
| 135 | + * link_transforms[i].rpy: the rotation component of T_L_i, in ROLL-PITCH-YAW XYZ convention |
| 136 | +
|
| 137 | + - "joints" is a vector containing information about each of the robot's joints. joints[i] contains |
| 138 | + the following information about the i-th joint of the robot (J_i): |
| 139 | + * joints[i].type: either 'fixed' or 'revolute'. A fixed joint does not move; it is meant to |
| 140 | + contain a static transform. If a joint is 'fixed', the transform associated with it is the identity. |
| 141 | + A 'revolute' joint on the other hand also has a joint angle that changes as the robot moves; |
| 142 | + this information is contained in the "joint_values" parameter explained below. |
| 143 | + * joints[i].axis: (only if type is 'revolute') the axis of rotation of joint J_i, expressed in the |
| 144 | + coordinate frame associated with the end of the previuos link (or the coordinate frame *after |
| 145 | + processing link L_{i-1}) |
| 146 | +
|
| 147 | + - "joint_values" contains the current joint values in the robot. In particular, joint_values[i] |
| 148 | + contains q_i, the value of the i-th joint of the robot. The joints in this list are in the same order |
| 149 | + as in the "joints" variable. |
| 150 | +
|
| 151 | + The goal of this function is to return the list of transforms between the base coordinate frame and |
| 152 | + each of the robot's links. You must return this information in the transforms[] list, which must |
| 153 | + contain the appropriate number of entries. As described in the homework handout, transforms[k] must |
| 154 | + contain the transform between the base coordinate system and coordinate system {k}. |
| 155 | + """ |
| 156 | + def forward_kinematics(self, link_transforms, joints, joint_values): |
| 157 | + transforms = [] |
| 158 | +#Step 0: create a series of lists that will hold matricies |
| 159 | + J_rot_list=[] |
| 160 | + L_comp_list=[] |
| 161 | + RL_comp_list=[] |
| 162 | + recourse_list=[numpy.identity(4)] |
| 163 | +#Step 1: create for loop that will cycle through links and joints and create transformation matricies |
| 164 | + for i in range(len(joints)): |
| 165 | + |
| 166 | + if joints[i].type == "fixed": #Create contingencies for fixed joints |
| 167 | + J_rot=numpy.identity(4) |
| 168 | + else: #Create joint transformation using q and axis |
| 169 | + J_rot=tf.transformations.rotation_matrix(joint_values[i], joints[i].axis) |
| 170 | + J_rot_list.append(J_rot) |
| 171 | + |
| 172 | + #Create link transformations using rpy and xyz translations |
| 173 | + L_trans=tf.transformations.translation_matrix(link_transforms[i].xyz) |
| 174 | + L_rot=tf.transformations.euler_matrix(link_transforms[i].rpy[0], link_transforms[i].rpy[1],link_transforms[i].rpy[2], 'rxyz') |
| 175 | + L_comp=numpy.dot(L_trans,L_rot) |
| 176 | + L_comp_list.append(L_comp) |
| 177 | + #Dot joint(i) and link(i) |
| 178 | + RL_comp=numpy.dot(J_rot_list[i],L_comp_list[i]) |
| 179 | + RL_comp_list.append(RL_comp) |
| 180 | +#Step 2: append transformaitons list with first terms 0 and 1 |
| 181 | + L_T_0=J_rot_list[0] |
| 182 | + L_T_1=numpy.dot(RL_comp_list[0], J_rot_list[1]) |
| 183 | + transforms.append(L_T_0) |
| 184 | + transforms.append(L_T_1) |
| 185 | + |
| 186 | + |
| 187 | +#Step 3: append recursive list with first term joint(0) and link(0) |
| 188 | + recourse_list.append(RL_comp_list[0]) |
| 189 | +#Step 4: create loop that will recusively update list with incrementally |
| 190 | + for i in range(2,len(joints)): |
| 191 | + recourse=numpy.dot(recourse_list[i-1],RL_comp_list[i-1]) |
| 192 | + recourse_list.append(recourse) |
| 193 | + L_T=numpy.dot(recourse_list[i], J_rot_list[i]) |
| 194 | + transforms.append(L_T) |
| 195 | +#Step 5: return transforms list |
| 196 | + print transforms |
| 197 | + return transforms |
| 198 | + |
| 199 | + |
| 200 | +if __name__ == '__main__': |
| 201 | + rospy.init_node('fwk', anonymous=True) |
| 202 | + fwk = ForwardKinematics() |
| 203 | + rospy.spin() |
0 commit comments