def joints_to_kdl(self, type, values=None, last_joint=None):
if values is None:
if type == 'positions':
cur_type_values = self.joint_angles
elif type == 'velocities':
cur_type_values = self.joint_velocities
elif type == 'torques':
cur_type_values = self.joint_efforts
else:
cur_type_values = values
if last_joint is None:
last_idx = len(self.joint_names) - 1
kdl_array = PyKDL.JntArray(self.n_joints)
else:
if last_joint not in self.joint_names:
raise rospy.ROSException('Invalid joint name, joint_name=' +
str(last_joint))
last_idx = self.joint_names.index(last_joint)
kdl_array = PyKDL.JntArray(last_idx + 1)
for idx in range(last_idx + 1):
name = self.joint_names[idx]
if name in cur_type_values:
kdl_array[idx] = cur_type_values[name]
if type == 'velocities':
kdl_array = PyKDL.JntArrayVel(kdl_array)
return kdl_array
def joints_to_kdl(self, type, values=None):
kdl_array = PyKDL.JntArray(self._num_jnts)
if values is None:
# print("RUDI: right now, the joint state always has to be passed")
cur_type_values = dict(zip(self._joint_names,[0.0]*self._num_jnts))
#
# if type == 'positions':
# cur_type_values = self._limb_interface.joint_angles()
# elif type == 'velocities':
# cur_type_values = self._limb_interface.joint_velocities()
# elif type == 'torques':
# cur_type_values = self._limb_interface.joint_efforts()
elif isinstance(values,(list, tuple)):
cur_type_values = dict(zip(self._joint_names,values))
elif isinstance(values, np.ndarray):
cur_type_values = dict(zip(self._joint_names,values.tolist()))
else:
raise ValueError("Unsupported type for values: {}".format(type))
for idx, name in enumerate(self._joint_names):
kdl_array[idx] = cur_type_values[name]
if type == 'velocities':
kdl_array = PyKDL.JntArrayVel(kdl_array)
return kdl_array
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 joints_to_kdl(self, type_, values=None):
kdl_array = PyKDL.JntArray(self._num_jnts)
pos_array = PyKDL.JntArray(self._num_jnts)
if values is None:
if type_ == 'positions':
cur_type_values = self._limb_interface.joint_angles()
elif type_ == 'velocities':
cur_type_values = self._limb_interface.joint_velocities()
pos_list = self._limb_interface.joint_angles()
elif type_ == 'torques':
cur_type_values = self._limb_interface.joint_efforts()
else:
cur_type_values = values
for idx, name in enumerate(self._joint_names):
kdl_array[idx] = cur_type_values[name]
if type_ == 'velocities':
pos_array[idx] = pos_list[name]
if type_ == 'velocities':
kdl_array = PyKDL.JntArrayVel(
pos_array, kdl_array
) # ----- using different constructor for getting velocity fk
return kdl_array
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 joints_to_kdl(self, values, type='positions'):
'''
'''
kdl_array = PyKDL.JntArray(self.num_joints)
for idx, name in enumerate(self.joint_names):
kdl_array[idx] = values[idx]
if type == 'velocities':
kdl_array = PyKDL.JntArrayVel(kdl_array)
return kdl_array
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 joints_to_kdl(self, type, values):
kdl_array = PyKDL.JntArray(self._num_jnts)
cur_type_values = values
for idx in range(self._num_jnts):
kdl_array[idx] = cur_type_values[idx]
if type == 'velocities':
kdl_array = PyKDL.JntArrayVel(kdl_array)
return kdl_array
def joints_to_kdl(self, type='positions', values=None):
kdl_array = PyKDL.JntArray(self._num_joints)
if values is None:
values = np.zeros((self._num_joints, 1))
for idx, name in enumerate(self._joint_names):
kdl_array[idx] = values[idx]
if type == 'velocities':
kdl_array = PyKDL.JntArrayVel(kdl_array)
return kdl_array
def __init__(self):
self.base_link = 'base_link'
self.ee_link = 'ee_link'
flag, self.tree = kdl_parser.treeFromParam('/robot_description')
self.chain = self.tree.getChain(self.base_link, self.ee_link)
self.num_joints = self.tree.getNrOfJoints()
self.vel_ik_solver = kdl.ChainIkSolverVel_pinv(self.chain)
self.pos_fk_solver = kdl.ChainFkSolverPos_recursive(self.chain)
self.pos_ik_solver = kdl.ChainIkSolverPos_LMA(self.chain)
self.cam_model = image_geometry.PinholeCameraModel()
self.joint_state = kdl.JntArrayVel(self.num_joints)
self.joint_names = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
rospy.init_node('ur_eef_vel_controller')
rospy.Subscriber('/joint_states', JointState, self.arm_joint_state_cb)
rospy.Subscriber('/rdda_interface/joint_states', JointState,
self.finger_joint_state_cb)
self.joint_vel_cmd_pub = rospy.Publisher(
'/joint_group_vel_controller/command',
Float64MultiArray,
queue_size=10)
self.joint_pos_cmd_pub = rospy.Publisher(
'/scaled_pos_traj_controller/command',
JointTrajectory,
queue_size=10)
self.speed_scaling_pub = rospy.Publisher('/speed_scaling_factor',
Float64,
queue_size=10)
self.switch_controller_cli = rospy.ServiceProxy(
'/controller_manager/switch_controller', SwitchController)
self.joint_pos_cli = actionlib.SimpleActionClient(
'/scaled_pos_joint_traj_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
cam_info = rospy.wait_for_message('/camera/depth/camera_info',
CameraInfo,
timeout=10)
self.cam_model.fromCameraInfo(cam_info)
self.bridge = CvBridge() # allows conversion from depth image to array
self.tf_listener = tf.TransformListener()
self.image_offset = 100
self.pos_ctrler_running = False
rospy.sleep(0.5)
def joints_to_kdl(self, type):
kdl_array = PyKDL.JntArray(self._num_jnts)
if type == 'positions':
cur_type_values = self._limb_interface.joint_angles()
elif type == 'velocities':
cur_type_values = self._limb_interface.joint_velocities()
elif type == 'torques':
cur_type_values = self._limb_interface.joint_efforts()
for idx, name in enumerate(self._joint_names):
kdl_array[idx] = cur_type_values[name]
if type == 'velocities':
kdl_array = PyKDL.JntArrayVel(kdl_array)
return kdl_array
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 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 joints_to_kdl(self, type, values=None):
kdl_array = PyKDL.JntArray(self.n_joints)
if values is None:
if type == 'positions':
cur_type_values = self.joint_angles
elif type == 'velocities':
cur_type_values = self.joint_velocities
elif type == 'torques':
cur_type_values = self.joint_efforts
else:
cur_type_values = values
for idx in range(len(self.joint_names)):
if self.joint_names[idx] in cur_type_values:
kdl_array[idx] = cur_type_values[self.joint_names[idx]]
if type == 'velocities':
kdl_array = PyKDL.JntArrayVel(kdl_array)
return kdl_array
def joints_to_kdl(self, type, values=None): #
kdl_array = PyKDL.JntArray(len(self._limb.joint_names()))
if values is None:
if type == 'positions':
cur_type_values = self._limb.joint_angles()
elif type == 'velocities':
cur_type_values = self._limb.joint_velocities()
elif type == 'torques':
cur_type_values = self._limb.joint_efforts()
else:
cur_type_values = values
for idx, name in enumerate(
self._limb.joint_names()): #changed from joint_names()
kdl_array[idx] = cur_type_values[name]
if type == 'velocities':
kdl_array = PyKDL.JntArrayVel(kdl_array)
return kdl_array
def joints_to_kdl(self, type, values=None):
kdl_array = PyKDL.JntArray(self._num_jnts)
if values is None:
raise Exception('Error in TKinematics.joints_to_kdl')
#if type == 'positions':
#cur_type_values = self._limb_interface.joint_angles()
#elif type == 'velocities':
#cur_type_values = self._limb_interface.joint_velocities()
#elif type == 'torques':
#cur_type_values = self._limb_interface.joint_efforts()
else:
cur_type_values = values
for idx, name in enumerate(self.joint_names):
kdl_array[idx] = cur_type_values[name]
if type == 'velocities':
kdl_array = PyKDL.JntArrayVel(kdl_array)
return kdl_array
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 joints_to_kdl(self, type, values=None):
kdl_array = PyKDL.JntArray(self._num_jnts)
if values is None:
# if type == 'positions':
# cur_type_values = self._limb_interface.joint_angles()
# elif type == 'velocities':
# cur_type_values = self._limb_interface.joint_velocities()
# elif type == 'torques':
# cur_type_values = self._limb_interface.joint_efforts()
#cur_type_values = {'joint_a1': 0.0, 'joint_a2': -1.57, 'joint_a3': 1.57, 'joint_a4': 0.0, 'joint_a5': -1.57, 'joint_a6': 0.0}
self._joint_positions = [0.0, -1.57, 1.57, 0.0, 1.57,
0.0] # HARDCODED POSITION
cur_type_values = dict(
zip(self._joint_names, self._joint_positions))
else:
cur_type_values = values
for idx, name in enumerate(self._joint_names):
kdl_array[idx] = cur_type_values[name]
if type == 'velocities':
kdl_array = PyKDL.JntArrayVel(kdl_array)
return kdl_array
