def build_chain(self): self.chain = Chain() for e in self._config: v = Vector(e["xyzrpy"][0], e["xyzrpy"][1], e["xyzrpy"][2]) r = Rotation.RPY(e["xyzrpy"][3], e["xyzrpy"][4], e["xyzrpy"][5]) j = Joint(e["name"], Joint.None) f = Frame(r, v) if e["type"] == "rotx": axis = Vector(1.0, 0.0, 0.0) j = Joint(e["name"], f.p, f.M * axis, Joint.RotAxis) elif e["type"] == "roty": axis = Vector(0.0, 1.0, 0.0) j = Joint(e["name"], f.p, f.M * axis, Joint.RotAxis) elif e["type"] == "rotz": axis = Vector(0.0, 0.0, 1.0) j = Joint(e["name"], f.p, f.M * axis, Joint.RotAxis) elif e["type"] == "transx": axis = Vector(1.0, 0.0, 0.0) j = Joint(e["name"], f.p, f.M * axis, Joint.TransAxis) elif e["type"] == "transy": axis = Vector(0.0, 1.0, 0.0) j = Joint(e["name"], f.p, f.M * axis, Joint.TransAxis) elif e["type"] == "transz": axis = Vector(0.0, 0.0, 1.0) j = Joint(e["name"], f.p, f.M * axis, Joint.TransAxis) self.chain.addSegment( Segment(e["name"] + "seg", j, f, RigidBodyInertia())) self.fksolverpos = ChainFkSolverPos_recursive(self.chain)
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