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