wassname/kair_algorithms_draft
1
0
Fork 0
mirror of https://github.com/wassname/kair_algorithms_draft.git synced 2026-09-09 11:25:10 +08:00
Code Issues Packages Projects Releases Wiki Activity

Resolve the wrong kdl kinematics due to ill robot tree structure

Browse Source
This commit is contained in:
Cheolhui
2019-05-06 11:23:21 +09:00
parent ce430fb0f3
commit bf07b2b3c7
1 changed files with 2 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
scripts/envs/open_manipulator/ros_interface.py
+2 -2
View File
@@ -121,7 +121,7 @@ class OpenManipulatorRosBaseInterface(object):
def init_fk_solver(self):
self.robot = URDF.from_parameter_server()
self.solve_fk = KDLKinematics(self.robot, "link1", "end_effector_link")
self.solve_fk = KDLKinematics(self.robot, "world", "end_effector_link")
def joint_state_callback(self, msg):
"""Callback function of joint states subscriber.
@@ -136,7 +136,7 @@ class OpenManipulatorRosBaseInterface(object):
self.joint_efforts = joints_states.effort
# penalize jerky motion in reward for shaped reward setting.
self.squared_sum_vel = np.linalg.norm(np.array(self.joint_velocities))
_fk_mat = np.array(self.solve_fk.forward(self.joint_positions[:4]))
_fk_mat = np.array(self.solve_fk.forward(self.joint_positions[2:]))
self._gripper_position = _fk_mat[0:3, 3]
self._gripper_orientation[3] = (1 + _fk_mat[0, 0] +
_fk_mat[1, 1] + _fk_mat[2, 2])**0.5