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
