def forward_velocity_kinematics(self, joint_velocities=None):
"""Solve forward vel-kine : joint-vel -> ee-vels
"""
end_frame = PyKDL.FrameVel()
self._fk_v_kdl.JntToCart(
self.joints_to_kdl('velocities', joint_velocities), end_frame)
return end_frame.GetTwist()
def compute_fk_velocity(self, joint_positions,
joint_velocities, des_frame):
"""
Given joint_positions and joint velocities,
compute the velocities of des_frame with respect
to the base frame
:param joint_positions: joint positions
:param joint_velocities: joint velocities
:param des_frame: end frame
:type joint_positions: np.ndarray
:type joint_velocities: np.ndarray
:type des_frame: string
:return: translational and rotational
velocities (vx, vy, vz, wx, wy, wz)
:rtype: np.ndarray
"""
kdl_end_frame = kdl.FrameVel()
kdl_jnt_angles = prutil.joints_to_kdl(joint_positions)
kdl_jnt_vels = prutil.joints_to_kdl(joint_velocities)
kdl_jnt_qvels = kdl.JntArrayVel(kdl_jnt_angles, kdl_jnt_vels)
idx = self.arm_link_names.index(des_frame) + 1
fg = self.fk_solver_vel.JntToCart(kdl_jnt_qvels,
kdl_end_frame,
idx)
assert fg == 0, 'KDL Vel JntToCart error!'
end_twist = kdl_end_frame.GetTwist()
return np.array([end_twist[0], end_twist[1], end_twist[2],
end_twist[3], end_twist[4], end_twist[5]])
def compute_fk_velocity(self, jpos, jvel, tgt_frame):
"""
Given joint_positions and joint velocities,
compute the velocities of tgt_frame with respect
to the base frame.
Args:
jpos (list or flattened np.ndarray): joint positions.
jvel (list or flattened np.ndarray): joint velocities.
tgt_frame (str): target link frame.
Returns:
np.ndarray: translational velocity and rotational velocity
(vx, vy, vz, wx, wy, wz) (shape: :math:`[6,]`).
"""
if isinstance(jpos, list):
jpos = np.array(jpos)
if isinstance(jvel, list):
jvel = np.array(jvel)
kdl_end_frame = kdl.FrameVel()
kdl_jnt_angles = joints_to_kdl(jpos)
kdl_jnt_vels = joints_to_kdl(jvel)
kdl_jnt_qvels = kdl.JntArrayVel(kdl_jnt_angles, kdl_jnt_vels)
idx = self.arm_link_names.index(tgt_frame) + 1
fg = self._fk_solver_vel.JntToCart(kdl_jnt_qvels,
kdl_end_frame,
idx)
if fg < 0:
raise ValueError('KDL Vel JntToCart error!')
end_twist = kdl_end_frame.GetTwist()
return np.array([end_twist[0], end_twist[1], end_twist[2],
end_twist[3], end_twist[4], end_twist[5]])
def forward_velocity_kinematics(self,
joint_values=None,
joint_velocities=None):
end_frame = PyKDL.FrameVel()
kdl_JntArray_pos = self.joints_to_kdl('positions', joint_values)
kdl_JntArray_vel = self.joints_to_kdl('velocities', joint_velocities)
kdl_JntArrayVel = PyKDL.JntArrayVel(kdl_JntArray_pos, kdl_JntArray_vel)
self._fk_v_kdl.JntToCart(kdl_JntArrayVel, end_frame)
return end_frame.GetTwist()
def joint_states_cb(self, msg):
state_msg = EndpointState()
# Forward Position Kinematics
q, qd, eff = self.kinematics.extract_joint_state(msg)
T06 = self.kinematics.forward(q)
state_msg.pose = PoseConv.to_pose_msg(T06)
# Forward Velocity Kinematics
end_frame = PyKDL.FrameVel()
q_vel = PyKDL.JntArrayVel(joint_list_to_kdl(q), joint_list_to_kdl(qd))
self.fk_vel_solver.JntToCart(q_vel, end_frame)
state_msg.twist = TwistKDLToMsg(end_frame.GetTwist())
state_msg.header.frame_id = self.frame_id
state_msg.header.stamp = rospy.Time.now()
try:
self.state_pub.publish(state_msg)
except:
pass
def kdl_inv_vel_kine(self, cur_joint_pos, ee_twist=None):
""" inverse velocity kinematics using the solver in PyKDL
instance : self.ik_v_kdl
refer ik_p_kdl-> ik_p_kdl.CartToJnt(seed_array, goal_pose, result_angles) >= 0:
PyKDL -> ee frame velocity?
KDL::FrameVel v_out;
tf::poseMsgToKDL(cart_pose, pose_kdl);
// cur_jnt_position, ee_vel
CartToJntVel(JointVelList[i].q, end_effector_vel.GetTwist(), NullSpaceBias[i], result_vel)
kin.ik_solver->CartTo
"""
frame_vel = PyKDL.FrameVel()
_joint_vels = self.joints_to_kdl(type='velocities', values=None)
# args : CartToJnt-> (cur_joint_pos, targ_ee_vel, result_joint_vel)
if self.ik_v_kdl.CartToJnt(cur_joint_pos, ee_twist, _joint_vels) >= 0:
return list(_joint_vels)
else:
return None
def forward_velocity_kinematics(self, joint_values=None, joint_velocities=None):
if joint_values is None:
joint_values = self.joint_angles
if joint_velocities is None:
joint_values = self.joint_velocities
q = PyKDL.JntArray(self.n_joints)
q_dot = PyKDL.JntArray(self.n_joints)
for idx in range(len(self.joint_names)):
q[idx] = joint_values[self.joint_names[idx]]
q_dot[idx] = joint_velocities[self.joint_names[idx]]
vel_array = PyKDL.JntArrayVel(q, q_dot)
end_frame = PyKDL.FrameVel()
self._fk_v_kdl.JntToCart(vel_array, end_frame)
return end_frame.GetTwist()
def update_ee_state(self, joint_pos, joint_vel):
end_frame = PyKDL.Frame()
kdl_array = PyKDL.JntArray(len(joint_pos))
for idx, val in enumerate(joint_pos):
kdl_array[idx] = val
self._fk_p_kdl.JntToCart(kdl_array, end_frame)
self._ee_frame = end_frame
pos = end_frame.p
rot = PyKDL.Rotation(end_frame.M)
rot = rot.GetQuaternion()
self._ee_pose = np.array([pos[0], pos[1], pos[2], rot[0], rot[1], rot[2], rot[3]])
end_frame = PyKDL.FrameVel()
kdl_array = PyKDL.JntArray(len(joint_vel))
for idx, val in enumerate(joint_vel):
kdl_array[idx] = val
kdl_array = PyKDL.JntArrayVel(kdl_array)
self._fk_v_kdl.JntToCart(kdl_array, end_frame)
self._ee_twist = end_frame.GetTwist() #not numpy array...
def forward_velocity_kinematics(self, joint_velocities=None):
end_frame = PyKDL.FrameVel()
self._fk_v_kdl.JntToCart(
self.joints_to_kdl('velocities2', joint_velocities), end_frame)
return end_frame.GetTwist()
