def __init__(self, urdf_param='robot_description'): self._urdf = URDF.from_parameter_server(urdf_param) (parse_ok, self._kdl_tree) = treeFromUrdfModel(self._urdf) # Check @TODO Exception if not parse_ok: rospy.logerr( 'Error parsing URDF from parameter server ({0})'.format( urdf_param)) else: rospy.logdebug('Parsing URDF succesful') self._base_link = self._urdf.get_root() # @TODO Hardcoded self._tip_link = 'link_6' self._tip_frame = PyKDL.Frame() self._arm_chain = self._kdl_tree.getChain(self._base_link, self._tip_link) # @TODO Hardcoded self._joint_names = ['a1', 'a2', 'a3', 'a4', 'a5', 'a6'] self._num_joints = len(self._joint_names) # KDL Solvers self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain) self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._arm_chain) self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain) self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain, self._fk_p_kdl, self._ik_v_kdl) self._jac_kdl = PyKDL.ChainJntToJacSolver(self._arm_chain) self._dyn_kdl = PyKDL.ChainDynParam(self._arm_chain, PyKDL.Vector.Zero())
def __init__(self, limb, description=None): if description is None: self._franka = URDF.from_parameter_server(key='robot_description') else: self._franka = URDF.from_xml_file(description) self._kdl_tree = kdl_tree_from_urdf_model(self._franka) self._base_link = self._franka.get_root() self._tip_link = limb.name + ('_hand' if limb.has_gripper else '_link8') self._tip_frame = PyKDL.Frame() self._arm_chain = self._kdl_tree.getChain(self._base_link, self._tip_link) self._limb_interface = limb self._joint_names = deepcopy(self._limb_interface.joint_names()) self._num_jnts = len(self._joint_names) # KDL Solvers self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain) self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._arm_chain) self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain) self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain, self._fk_p_kdl, self._ik_v_kdl) self._jac_kdl = PyKDL.ChainJntToJacSolver(self._arm_chain) self._dyn_kdl = PyKDL.ChainDynParam(self._arm_chain, PyKDL.Vector.Zero())
def __inverse_kinematics(self, position, orientation=None, seed=None): """inverse kinematic solver using PyKDL""" _inverse_kin_position_kdl = PyKDL.ChainIkSolverPos_NR_JL(self.iiwa_chain, self.min_limits, self.max_limits, self.forward_kin_position_kdl, self.inverse_kin_velocity_kdl) ik = PyKDL.ChainIkSolverVel_pinv(self.iiwa_chain) pos = PyKDL.Vector(position[0], position[1], position[2]) if orientation != None: rot = PyKDL.Rotation() #PyKDL uses w, x, y, z instead of x, y, z, w rot = rot.Quaternion(orientation[1], orientation[2], orientation[3], orientation[0]) seed_array = PyKDL.JntArray(NUM_JOINTS) if seed != None: seed_array.resize(len(seed)) for idx, jnt in enumerate(seed): seed_array[idx] = jnt else: joint_vals = self.group.get_current_joint_values() for idx, jnt in enumerate(joint_vals): seed_array[idx] = joint_vals[idx] if orientation: goal_pose = PyKDL.Frame(rot, pos) else: goal_pose = PyKDL.Frame(pos) result_angles = PyKDL.JntArray(NUM_JOINTS) if _inverse_kin_position_kdl.CartToJnt(seed_array, goal_pose, result_angles) >= 0: result = np.array(result_angles) return result else: print 'No IK Solution Found' return None
def inverse_kinematics(self, position, orientation=None, seed=None): ik = PyKDL.ChainIkSolverVel_pinv(self._kdl_chain) pos = PyKDL.Vector(position[0], position[1], position[2]) if orientation is not None: rot = PyKDL.Rotation() rot = rot.Quaternion(orientation[0], orientation[1], orientation[2], orientation[3]) # Populate seed with current angles if not provided seed_array = PyKDL.JntArray(self._num_jnts) if seed is not None: seed_array.resize(len(seed)) for idx, jnt in enumerate(seed): seed_array[idx] = jnt else: seed_array = self._joints_to_kdl() # Make IK Call if orientation is not None: goal_pose = PyKDL.Frame(rot, pos) else: goal_pose = PyKDL.Frame(pos) result_angles = PyKDL.JntArray(self._num_jnts) num_tries = 0 status = self._ik_p_kdl.CartToJnt(seed_array, goal_pose, result_angles) if status < 0: print "IK solution not found with current joint angles as seed. Trying random seeds..." while status < 0 and num_tries < 100: seed_array = self._get_random_seed_array() status = self._ik_p_kdl.CartToJnt(seed_array, goal_pose, result_angles) num_tries += 1 if status < 0: print "No IK solution found." return None else: return np.array(result_angles)
def __init__(self, robot, base_link=None, tip_link=None, data_path="../.."): cur_path = os.path.dirname(os.path.abspath(__file__)) if PYTHON2: import cPickle as pickle with open(cur_path + "/robot.pkl", "rb") as fid: robot_info = pickle.load(fid) else: import pickle with open(cur_path + "/robot_p3.pkl", "rb") as fid: robot_info = pickle.load(fid) self._pose_0 = robot_info["_pose_0"] self.finger_pose = self._pose_0[-2].copy() self._joint_origin = robot_info["_joint_axis"] self._tip2joint = robot_info["_tip2joint"] self._joint_axis = robot_info["_joint_axis"] self._joint_limits = robot_info["_joint_limits"] self._joint2tips = robot_info["_joint2tips"] self._joint_name = robot_info["_joint_name"] self.center_offset = np.array(robot_info["center_offset"]) self._link_names = robot_info["_link_names"] self._base_link, self._tip_link = "panda_link0", "panda_hand" self._num_jnts = 7 self._robot = URDF.from_xml_string( open( os.path.join(cur_path, data_path, "data/robots", "panda_arm_hand.urdf"), "r", ).read()) self._kdl_tree, _ = kdl_tree_from_urdf_model(self._robot) self.soft_joint_limit_padding = 0.2 mins_kdl = joint_list_to_kdl( np.array([ self._joint_limits[n][0] + self.soft_joint_limit_padding for n in self._joint_name[:-3] ])) maxs_kdl = joint_list_to_kdl( np.array([ self._joint_limits[n][1] - self.soft_joint_limit_padding for n in self._joint_name[:-3] ])) self._arm_chain = self._kdl_tree.getChain(self._base_link, self._tip_link) self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain) self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain) self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR_JL(self._arm_chain, mins_kdl, maxs_kdl, self._fk_p_kdl, self._ik_v_kdl)
def inverse_kinematics(self, position, orientation=None, seed=None): ik = PyKDL.ChainIkSolverVel_pinv(self._arm_chain) pos = PyKDL.Vector(position[0], position[1], position[2]) if orientation != None: rot = PyKDL.Rotation() rot = rot.Quaternion(orientation[0], orientation[1], orientation[2], orientation[3]) # Populate seed with current angles if not provided seed_array = PyKDL.JntArray(self._num_jnts) if seed != None: seed_array.resize(len(seed)) for idx, jnt in enumerate(seed): seed_array[idx] = jnt else: seed_array = self.joints_to_kdl('positions') # Make IK Call if orientation: goal_pose = PyKDL.Frame(rot, pos) else: goal_pose = PyKDL.Frame(pos) result_angles = PyKDL.JntArray(self._num_jnts) if self._ik_p_kdl.CartToJnt(seed_array, goal_pose, result_angles) >= 0: result = np.array(list(result_angles)) return result else: return None
def __init__(self, urdf, base_link, end_link, kdl_tree=None): if kdl_tree is None: kdl_tree = kdl_tree_from_urdf_model(urdf) self.tree = kdl_tree self.urdf = urdf base_link = base_link.split("/")[-1] # for dealing with tf convention end_link = end_link.split("/")[-1] # for dealing with tf convention self.chain = kdl_tree.getChain(base_link, end_link) self.base_link = base_link self.end_link = end_link # record joint information in easy-to-use lists self.joint_limits_lower = [] self.joint_limits_upper = [] self.joint_safety_lower = [] self.joint_safety_upper = [] self.joint_types = [] for jnt_name in self.get_joint_names(): jnt = urdf.joints[jnt_name] if jnt.limits is not None: self.joint_limits_lower.append(jnt.limits.lower) self.joint_limits_upper.append(jnt.limits.upper) else: self.joint_limits_lower.append(None) self.joint_limits_upper.append(None) if jnt.safety is not None: self.joint_safety_lower.append(jnt.safety.lower) self.joint_safety_upper.append(jnt.safety.upper) elif jnt.limits is not None: self.joint_safety_lower.append(jnt.limits.lower) self.joint_safety_upper.append(jnt.limits.upper) else: self.joint_safety_lower.append(None) self.joint_safety_upper.append(None) self.joint_types.append(jnt.joint_type) def replace_none(x, v): if x is None: return v return x self.joint_limits_lower = np.array( [replace_none(jl, -np.inf) for jl in self.joint_limits_lower]) self.joint_limits_upper = np.array( [replace_none(jl, np.inf) for jl in self.joint_limits_upper]) self.joint_safety_lower = np.array( [replace_none(jl, -np.inf) for jl in self.joint_safety_lower]) self.joint_safety_upper = np.array( [replace_none(jl, np.inf) for jl in self.joint_safety_upper]) self.joint_types = np.array(self.joint_types) self.num_joints = len(self.get_joint_names()) self._fk_kdl = kdl.ChainFkSolverPos_recursive(self.chain) self._ik_v_kdl = kdl.ChainIkSolverVel_pinv(self.chain) self._ik_p_kdl = kdl.ChainIkSolverPos_NR(self.chain, self._fk_kdl, self._ik_v_kdl) self._jac_kdl = kdl.ChainJntToJacSolver(self.chain) self._dyn_kdl = kdl.ChainDynParam(self.chain, kdl.Vector.Zero())
def get_kinematics_solvers(kdl_chain, kdl_limits_min, kdl_limits_max): fk_solver = PyKDL.ChainFkSolverPos_recursive(kdl_chain) velocity_ik = PyKDL.ChainIkSolverVel_pinv(kdl_chain) # ik_solver = PyKDL.ChainIkSolverPos_LMA(kdl_chain, 1e-5, 1000, 1e-15) ik_solver = PyKDL.ChainIkSolverPos_NR_JL(kdl_chain, kdl_limits_min, kdl_limits_max, fk_solver, velocity_ik) return fk_solver, ik_solver
def __init__(self): ns = [ item for item in rospy.get_namespace().split('/') if len(item) > 0 ] if len(ns) != 2: rospy.ROSException( 'The controller must be called in the manipulator namespace') self._namespace = ns[0] self._arm_name = ns[1] if self._namespace[0] != '/': self._namespace = '/' + self._namespace if self._namespace[-1] != '/': self._namespace += '/' # The arm interface loads the parameters from the URDF file and # parameter server and initializes the KDL tree self._arm_interface = ArmInterface() self._base_link = self._arm_interface.base_link self._tip_link = self._arm_interface.tip_link self._tip_frame = PyKDL.Frame() # KDL Solvers self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_interface.chain) self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR( self._arm_interface.chain, self._arm_interface._fk_p_kdl, self._ik_v_kdl, 100, 1e-6) self._dyn_kdl = PyKDL.ChainDynParam(self._arm_interface.chain, PyKDL.Vector.Zero()) # Add a callback function to calculate the end effector's state by each # update of the joint state self._arm_interface.add_callback('joint_states', self.on_update_endeffector_state) # Publish the current manipulability index at each update of the joint # states # self._arm_interface.add_callback('joint_states', # self.publish_man_index) # Publish the end effector state self._endeffector_state_pub = rospy.Publisher('endpoint_state', EndPointState, queue_size=1) # Publish the manipulability index self._man_index_pub = rospy.Publisher('man_index', Float64, queue_size=1) self._services = dict() # Provide the service to calculate the inverse kinematics using the KDL solver self._services['ik'] = rospy.Service('ik_solver', SolveIK, self.solve_ik)
def calculate_ik(base_link, tip_link, seed_joint_state, goal_transform_geometry_msg): """ Calculates the Inverse Kinematics from base_link to tip_link according to the given goal_transform_geometry_msg. The initial joint states would be considered from seed_joint_state. Returns the result joint states and the success status. base_link eg. - "triangle_base_link" or "calib_left_arm_base_link" or "calib_right_arm_base_link" tip_link eg. - "left_arm_7_link" or "right_arm_7_link" """ robot_urdf_string = rospy.get_param('robot_description') urdf_obj = urdf_parser_py.urdf.URDF.from_xml_string(robot_urdf_string) _, kdl_tree = kdl_parser_py.urdf.treeFromUrdfModel(urdf_obj) kdl_chain = kdl_tree.getChain(base_link, tip_link) num_joints = kdl_chain.getNrOfJoints() print "number of joints: " + str(num_joints) # Get Joint limits kdl_joint_limits_min, kdl_joint_limits_max = get_kdl_joint_limit_arrays( kdl_chain, urdf_obj) fk_solver = PyKDL.ChainFkSolverPos_recursive(kdl_chain) velocity_ik = PyKDL.ChainIkSolverVel_pinv(kdl_chain) # ik_solver = PyKDL.ChainIkSolverPos_LMA(kdl_chain, 1e-5, 1000, 1e-15) ik_solver = PyKDL.ChainIkSolverPos_NR_JL(kdl_chain, kdl_joint_limits_min, kdl_joint_limits_max, fk_solver, velocity_ik) # Getting the goal frame and seed state goal_frame_kdl = tf2_kdl.transform_to_kdl(goal_transform_geometry_msg) seed_joint_state_kdl = get_kdl_jnt_array_from_list(num_joints, seed_joint_state) # Solving IK result_joint_state_kdl = solve_ik(ik_solver, num_joints, seed_joint_state_kdl, goal_frame_kdl) # check if calculated joint state results in the correct end-effector position using FK goal_pose_reached = check_ik_result_using_fk(fk_solver, result_joint_state_kdl, goal_frame_kdl) # check if calculated joint state is within joint limits joints_within_limits = check_result_joints_are_within_limits( num_joints, result_joint_state_kdl, kdl_joint_limits_min, kdl_joint_limits_max) print "Result Joint State Within Limits: " + str(joints_within_limits) print "Can Reach Goal Pose With Solution: " + str(goal_pose_reached) result_joint_state_vector = get_list_from_kdl_jnt_array( num_joints, result_joint_state_kdl) goal_pose_reached_successfully = goal_pose_reached and joints_within_limits return result_joint_state_vector, goal_pose_reached_successfully
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 _set_solvers(self): """Initialize the solvers according to the stored base chain and tool""" self._complete_chain = kdl.Chain(self._base_chain) if self._tool_segment is not None: if isinstance(self._tool_segment, kdl.Segment): self._complete_chain.addSegment(self._tool_segment) else: raise Exception("Tool is not None, Chain or Segment") self._fk_solver_pos = kdl.ChainFkSolverPos_recursive( self._complete_chain) self._tv_solver = kdl.ChainIkSolverVel_pinv(self._complete_chain) self._ik_solver_pos = kdl.ChainIkSolverPos_NR(self._complete_chain, self._fk_solver_pos, self._tv_solver)
def __init__(self, root_link=None, tip_link="left_arm_7_link"): self._urdf = URDF.from_parameter_server(key='robot_description') self._kdl_tree = kdl_tree_from_urdf_model(self._urdf) self._base_link = self._urdf.get_root( ) if root_link is None else root_link self._tip_link = tip_link self._arm_chain = self._kdl_tree.getChain(self._base_link, self._tip_link) self._joint_names = self._urdf.get_chain(self._base_link, self._tip_link, links=False, fixed=False) self._num_jnts = len(self._joint_names) # KDL self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain) self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain) self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain, self._fk_p_kdl, self._ik_v_kdl) # trac_ik urdf_str = rospy.get_param('/robot_description') self.trac_ik_solver = IK(self._base_link, self._tip_link, urdf_string=urdf_str) self.joint_limits_lower = [] self.joint_limits_upper = [] self.joint_types = [] for jnt_name in self._joint_names: jnt = self._urdf.joint_map[jnt_name] if jnt.limit is not None: self.joint_limits_lower.append(jnt.limit.lower) self.joint_limits_upper.append(jnt.limit.upper) else: self.joint_limits_lower.append(None) self.joint_limits_upper.append(None) self.joint_types.append(jnt.type) self._default_seed = [ -0.3723750412464142, 0.02747996523976326, -0.7221865057945251, -1.69843590259552, 0.11076358705759048, 0.5450965166091919, 0.45358774065971375 ] # home_pos
def inverse_kinematics(self, position, orientation=None, seed=None, min_joints=None, max_joints=None, maxiter=500, eps=1.0e-6): ik = PyKDL.ChainIkSolverVel_pinv(self._arm_chain) pos = PyKDL.Vector(position[0], position[1], position[2]) if orientation is not None: rot = PyKDL.Rotation() rot = rot.Quaternion(orientation[0], orientation[1], orientation[2], orientation[3]) # Populate seed with current angles if not provided seed_array = PyKDL.JntArray(self._num_jnts) if seed is not None: seed_array.resize(len(seed)) for idx, jnt in enumerate(seed): seed_array[idx] = jnt else: seed_array = self.joints_to_kdl('positions') # Make IK Call if orientation is not None: goal_pose = PyKDL.Frame(rot, pos) else: goal_pose = PyKDL.Frame(pos) result_angles = PyKDL.JntArray(self._num_jnts) # Make IK solver with joint limits if min_joints is None: min_joints = self.joint_limits_lower if max_joints is None: max_joints = self.joint_limits_upper mins_kdl = PyKDL.JntArray(len(min_joints)) for idx, jnt in enumerate(min_joints): mins_kdl[idx] = jnt maxs_kdl = PyKDL.JntArray(len(max_joints)) for idx, jnt in enumerate(max_joints): maxs_kdl[idx] = jnt ik_p_kdl = PyKDL.ChainIkSolverPos_NR_JL(self._arm_chain, mins_kdl, maxs_kdl, self._fk_p_kdl, self._ik_v_kdl, maxiter, eps) if ik_p_kdl.CartToJnt(seed_array, goal_pose, result_angles) >= 0: result = np.array(list(result_angles)) return result else: return None
def inverse_kinematics(robot_chain, pos, rot, q_guess=None, min_joints=None, max_joints=None): """ Perform inverse kinematics Args: robot_chain: robot's chain object pos: 1 x 3 or 3 x 1 array of the end effector position. rot: 3 x 3 array of the end effector rotation q_guess: guess values for the joint positions min_joints: minimum value of the position for each joint max_joints: maximum value of the position for each joint Returns: list of joint positions or None (no solution) """ pos_kdl = kdl.Vector(pos[0], pos[1], pos[2]) rot_kdl = kdl.Rotation(rot[0, 0], rot[0, 1], rot[0, 2], rot[1, 0], rot[1, 1], rot[1, 2], rot[2, 0], rot[2, 1], rot[2, 2]) frame_kdl = kdl.Frame(rot_kdl, pos_kdl) num_joints = robot_chain.getNrOfJoints() min_joints = -np.pi * np.ones( num_joints) if min_joints is None else min_joints max_joints = np.pi * np.ones( num_joints) if max_joints is None else max_joints mins_kdl = joint_list_to_kdl(min_joints) maxs_kdl = joint_list_to_kdl(max_joints) fk_kdl = kdl.ChainFkSolverPos_recursive(robot_chain) ik_v_kdl = kdl.ChainIkSolverVel_pinv(robot_chain) ik_p_kdl = kdl.ChainIkSolverPos_NR_JL(robot_chain, mins_kdl, maxs_kdl, fk_kdl, ik_v_kdl) if q_guess == None: # use the midpoint of the joint limits as the guess lower_lim = np.where(np.isfinite(min_joints), min_joints, 0.) upper_lim = np.where(np.isfinite(max_joints), max_joints, 0.) q_guess = (lower_lim + upper_lim) / 2.0 q_guess = np.where(np.isnan(q_guess), [0.] * len(q_guess), q_guess) q_kdl = kdl.JntArray(num_joints) q_guess_kdl = joint_list_to_kdl(q_guess) if ik_p_kdl.CartToJnt(q_guess_kdl, frame_kdl, q_kdl) >= 0: return joint_kdl_to_list(q_kdl) else: return None
def cargarURDF(self): # filename = '/home/kaiser/WS_ROS/catkin_ws/src/abb_experimental-indigo-devel/abb_irb120_gazebo/urdf/modelo_exportado.urdf' # filename = '/home/kaiser/WS_ROS/catkin_ws/src/urdf_serp/urdf/IRB120_URDF_v2/robots/IRB120_URDF_v2.URDF' filename = '/home/kaiser/WS_ROS/catkin_ws/src/urdf_serp/urdf/irb120_peq/irb120_peq.urdf' robot = urdf_parser_py.urdf.URDF.from_xml_file(filename) (ok, tree) = kdl_parser_py.urdf.treeFromFile(filename) self.chain = tree.getChain("base_link", "link_6") print self.chain.getNrOfSegments() self.solverCinematicaDirecta = PyKDL.ChainFkSolverPos_recursive( self.chain) self.vik = PyKDL.ChainIkSolverVel_pinv(self.chain) self.ik = PyKDL.ChainIkSolverPos_NR(self.chain, self.solverCinematicaDirecta, self.vik) self.actualizar_IK([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
def initialize_kinematic_solvers(self): robot_description = rospy.get_param('/yumi/velocity_control/robot_description', '/robot_description') self._robot = URDF.from_parameter_server(key=robot_description) self._kdl_tree = kdl_tree_from_urdf_model(self._robot) # self._base_link = self._robot.get_root() self._ee_link = 'gripper_' +self._arm_name + '_base' #name of the frame we want self._ee_frame = PyKDL.Frame() self._ee_arm_chain = self._kdl_tree.getChain(self._base_link, self._ee_link) # KDL Solvers self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._ee_arm_chain) self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._ee_arm_chain) self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._ee_arm_chain) #self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain, self._fk_p_kdl, self._ik_v_kdl) self._ik_p_kdl = PyKDL.ChainIkSolverPos_LMA(self._ee_arm_chain) self._jac_kdl = PyKDL.ChainJntToJacSolver(self._ee_arm_chain) self._dyn_kdl = PyKDL.ChainDynParam(self._ee_arm_chain, PyKDL.Vector.Zero())
class MoveGroupLeftArm(object): def __init__(self): #----------------control init begin----------------------------------------------- super(MoveGroupLeftArm, self).__init__() moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('iiwa_move_to_ee_pose', anonymous=True) self.robot = moveit_commander.RobotCommander() self.scene = moveit_commander.PlanningSceneInterface() group_name = "manipulator" self.group = moveit_commander.MoveGroupCommander(group_name) #group.set_max_velocity_scaling_factor(0.15) self.group.set_max_acceleration_scaling_factor(0.1) if self.group.set_planner_id(PLANNER_ID): print "Using planner: %s" % PLANNER_ID self.group.set_num_planning_attempts(100) self.group.set_planning_time(3) self.group.set_start_state_to_current_state() self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) self.gripper_io_publisher = rospy.Publisher('command/GripperState', iiwa_msgs.msg.GripperState, queue_size=10) self.upright_constraints = Constraints() try: rospy.wait_for_service('configuration/setSmartServoLimits', 1) except: print 'Service call timeout' try: self.set_speed_limits = rospy.ServiceProxy('configuration/setSmartServoLimits', SetSmartServoJointSpeedLimits) response = self.set_speed_limits(0.3, 0.1, -1) print 'Velocity limit set' print response except rospy.ServiceException, e: print "service call failed: %s"%e #----------------kinematics init begin--------------------------------------------- _iiwa_URDF = URDF.from_parameter_server(key='robot_description') _iiwa_kdl_tree = kdl_tree_from_urdf_model(_iiwa_URDF) _iiwa_base_link = _iiwa_URDF.get_root() self.iiwa_chain = _iiwa_kdl_tree.getChain(_iiwa_base_link, 'tool_link_ee') self.forward_kin_position_kdl = PyKDL.ChainFkSolverPos_recursive(self.iiwa_chain) _forward_kin_velocity_kdl = PyKDL.ChainFkSolverVel_recursive(self.iiwa_chain) self.inverse_kin_velocity_kdl = PyKDL.ChainIkSolverVel_pinv(self.iiwa_chain) self.min_limits = PyKDL.JntArray(NUM_JOINTS) self.max_limits = PyKDL.JntArray(NUM_JOINTS) for idx, jnt in enumerate(MIN_JOINT_LIMITS_DEG): self.min_limits[idx] = math.radians(jnt) for idx, jnt in enumerate(MAX_JOINT_LIMITS_DEG): self.max_limits[idx] = math.radians(jnt) self.component_map = {}
def __init__(self, limb, ee_frame_name="panda_link8", additional_segment_config=None, description=None): if description is None: self._franka = URDF.from_parameter_server(key='robot_description') else: self._franka = URDF.from_xml_file(description) self._kdl_tree = kdl_tree_from_urdf_model(self._franka) if additional_segment_config is not None: for c in additional_segment_config: q = quaternion.from_rotation_matrix(c["origin_ori"]).tolist() kdl_origin_frame = PyKDL.Frame( PyKDL.Rotation.Quaternion(q.x, q.y, q.z, q.w), PyKDL.Vector(*(c["origin_pos"].tolist()))) kdl_sgm = PyKDL.Segment(c["child_name"], PyKDL.Joint(c["joint_name"]), kdl_origin_frame, PyKDL.RigidBodyInertia()) self._kdl_tree.addSegment(kdl_sgm, c["parent_name"]) self._base_link = self._franka.get_root() self._tip_link = ee_frame_name self._tip_frame = PyKDL.Frame() self._arm_chain = self._kdl_tree.getChain(self._base_link, self._tip_link) self._limb_interface = limb self._joint_names = deepcopy(self._limb_interface.joint_names()) self._num_jnts = len(self._joint_names) # KDL Solvers self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain) self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._arm_chain) self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain) self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain, self._fk_p_kdl, self._ik_v_kdl) self._jac_kdl = PyKDL.ChainJntToJacSolver(self._arm_chain) self._dyn_kdl = PyKDL.ChainDynParam(self._arm_chain, PyKDL.Vector.Zero())
def main(): filename = None if (sys.argv > 1): filename = 'uthai_description/urdf/uthai.urdf' (ok, tree) = kdl_parser_py.urdf.treeFromFile(filename) chain = tree.getChain("base_footprint","r_foot_ft_link") fksolver = kdl.ChainFkSolverPos_recursive(chain) ikvelsolver = kdl.ChainIkSolverVel_pinv(chain) iksolver = kdl.ChainIkSolverPos_NR(chain,fksolver,ikvelsolver) q_init = kdl.JntArray(6) q = kdl.JntArray(6) F_dest = kdl.Frame(kdl.Rotation.RPY(0,0,0),kdl.Vector(0.0,0,0)) status = iksolver.CartToJnt(q_init,F_dest,q) print("Status : ",status) print(q)
def create_chain(self, ee_frame_name): self._tip_link = ee_frame_name self._arm_chain = self._kdl_tree.getChain(self._base_link, self._tip_link) self._joint_names = deepcopy(self._limb_interface.joint_names()) self._num_jnts = len(self._joint_names) # KDL Solvers self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain) self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._arm_chain) self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain) self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain, self._fk_p_kdl, self._ik_v_kdl) self._jac_kdl = PyKDL.ChainJntToJacSolver(self._arm_chain) self._dyn_kdl = PyKDL.ChainDynParam(self._arm_chain, PyKDL.Vector.Zero())
def inverseKinematics(robotChain, pos, rot, qGuess=None, minJoints=None, maxJoints=None): """ Perform inverse kinematics Args: robotChain: robot's chain object pos: 1 x 3 or 3 x 1 array of the end effector position. rot: 3 x 3 array of the end effector rotation qGuess: guess values for the joint positions minJoints: minimum value of the position for each joint maxJoints: maximum value of the position for each joint Returns: list of joint positions or None (no solution) """ # print("inside inverse: ", pos, " ; ", rot) posKdl = kdl.Vector(pos[0], pos[1], pos[2]) rotKdl = kdl.Rotation(rot[0, 0], rot[0, 1], rot[0, 2], rot[1, 0], rot[1, 1], rot[1, 2], rot[2, 0], rot[2, 1], rot[2, 2]) frameKdl = kdl.Frame(rotKdl, posKdl) numJoints = robotChain.getNrOfJoints() minJoints = -np.pi * np.ones(numJoints) if minJoints is None else minJoints maxJoints = np.pi * np.ones(numJoints) if maxJoints is None else maxJoints minsKdl = jointListToKdl(minJoints) maxsKdl = jointListToKdl(maxJoints) fkKdl = kdl.ChainFkSolverPos_recursive(robotChain) ikVKdl = kdl.ChainIkSolverVel_pinv(robotChain) ikPKdl = kdl.ChainIkSolverPos_NR_JL(robotChain, minsKdl, maxsKdl, fkKdl, ikVKdl) if qGuess is None: # use the midpoint of the joint limits as the guess lowerLim = np.where(np.isfinite(minJoints), minJoints, 0.) upperLim = np.where(np.isfinite(maxJoints), maxJoints, 0.) qGuess = (lowerLim + upperLim) / 2.0 qGuess = np.where(np.isnan(qGuess), [0.]*len(qGuess), qGuess) qKdl = kdl.JntArray(numJoints) qGuessKdl = jointListToKdl(qGuess) if ikPKdl.CartToJnt(qGuessKdl, frameKdl, qKdl) >= 0: return jointKdlToList(qKdl) else: return None
def __init__(self, rospy_init=False): if rospy_init: rospy.init_node('baxter_kinematics') # KDL chain from URDF self._urdf = URDF.from_parameter_server(key='robot_description') self._base_link = 'left_arm_mount' #self._urdf.get_root() self._tip_link = 'left_wrist' self._kdl_tree = kdl_tree_from_urdf_model(self._urdf) self._kdl_chain = self._kdl_tree.getChain(self._base_link, self._tip_link) # KDL Solvers self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._kdl_chain) self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._kdl_chain) self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._kdl_chain, self._fk_p_kdl, self._ik_v_kdl) # Baxter Interface Limb Instances self._limb = baxter_interface.Limb('left') self._joint_names = self._limb.joint_names() self._joint_names = [jn for jn in self._joint_names] # left_e0 is fixed self._num_jnts = len(self._joint_names)
def __init__(self, limb): self._sawyer = URDF.from_parameter_server(key='robot_description') self._kdl_tree = kdl_tree_from_urdf_model(self._sawyer) self._base_link = self._sawyer.get_root() self._tip_link = limb + '_hand' self._tip_frame = PyKDL.Frame() self._arm_chain = self._kdl_tree.getChain(self._base_link, self._tip_link) # Sawyer Interface Limb Instances self._limb_interface = intera_interface.Limb(limb) self._joint_names = self._limb_interface.joint_names() self._num_jnts = len(self._joint_names) # KDL Solvers self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain) self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._arm_chain) self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain) self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain, self._fk_p_kdl, self._ik_v_kdl) self._jac_kdl = PyKDL.ChainJntToJacSolver(self._arm_chain) self._dyn_kdl = PyKDL.ChainDynParam(self._arm_chain, PyKDL.Vector.Zero())
def __init__(self, T=20, weight=[1.0,1.0]): #initialize model self.chain = PyKDL.Chain() self.chain.addSegment(PyKDL.Segment("Base", PyKDL.Joint(PyKDL.Joint.None), PyKDL.Frame(PyKDL.Vector(0.0, 0.0, 0.042)), PyKDL.RigidBodyInertia(0.08659, PyKDL.Vector(0.00025, 0.0, -0.02420), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0)))) self.chain.addSegment(PyKDL.Segment("Joint1", PyKDL.Joint(PyKDL.Joint.RotZ), PyKDL.Frame(PyKDL.Vector(0.0, -0.019, 0.028)), PyKDL.RigidBodyInertia(0.00795, PyKDL.Vector(0.0, 0.019, -0.02025), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0)))) self.chain.addSegment(PyKDL.Segment("Joint2", PyKDL.Joint(PyKDL.Joint.RotY), PyKDL.Frame(PyKDL.Vector(0.0, 0.019, 0.0405)), PyKDL.RigidBodyInertia(0.09312, PyKDL.Vector(0.00000, -0.00057, -0.02731), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0)))) self.chain.addSegment(PyKDL.Segment("Joint3", PyKDL.Joint(PyKDL.Joint.RotZ), PyKDL.Frame(PyKDL.Vector(0.024, -0.019, 0.064)), PyKDL.RigidBodyInertia(0.19398, PyKDL.Vector(-0.02376, 0.01864, -0.02267), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0)))) self.chain.addSegment(PyKDL.Segment("Joint4", PyKDL.Joint("minus_RotY", PyKDL.Vector(0,0,0), PyKDL.Vector(0,-1,0), PyKDL.Joint.RotAxis), PyKDL.Frame(PyKDL.Vector(0.064, 0.019, 0.024)), PyKDL.RigidBodyInertia(0.09824, PyKDL.Vector(-0.02099, 0.0, -0.01213), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0)))) self.chain.addSegment(PyKDL.Segment("Joint5", PyKDL.Joint(PyKDL.Joint.RotX), PyKDL.Frame(PyKDL.Vector(0.0405, -0.019, 0.0)), PyKDL.RigidBodyInertia(0.09312, PyKDL.Vector(-0.01319, 0.01843, 0.0), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0)))) self.chain.addSegment(PyKDL.Segment("Joint6", PyKDL.Joint("minus_RotY", PyKDL.Vector(0,0,0), PyKDL.Vector(0,-1,0), PyKDL.Joint.RotAxis), PyKDL.Frame(PyKDL.Vector(0.064, 0.019, 0.0)), PyKDL.RigidBodyInertia(0.09824, PyKDL.Vector(-0.02099, 0.0, 0.01142), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0)))) self.chain.addSegment(PyKDL.Segment("Joint7", PyKDL.Joint(PyKDL.Joint.RotX), PyKDL.Frame(PyKDL.Vector(0.14103, 0.0, 0.0)), PyKDL.RigidBodyInertia(0.26121, PyKDL.Vector(-0.09906, 0.00146, -0.00021), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0)))) self.min_position_limit = [-160.0, -90.0, -160.0, -90.0, -160.0, -90.0, -160.0] self.max_position_limit = [160.0, 90.0, 160.0, 90.0, 160.0, 90.0, 160.0] self.min_joint_position_limit = PyKDL.JntArray(7) self.max_joint_position_limit = PyKDL.JntArray(7) for i in range (0,7): self.min_joint_position_limit[i] = self.min_position_limit[i]*pi/180 self.max_joint_position_limit[i] = self.max_position_limit[i]*pi/180 self.fksolver = PyKDL.ChainFkSolverPos_recursive(self.chain) self.iksolver = PyKDL.ChainIkSolverVel_pinv(self.chain) self.iksolverpos = PyKDL.ChainIkSolverPos_NR_JL(self.chain, self.min_joint_position_limit, self.max_joint_position_limit, self.fksolver, self.iksolver, 100, 1e-6) #parameter self.T = T self.weight_u = weight[0] self.weight_x = weight[1] self.nj = self.chain.getNrOfJoints() self.q_init = np.zeros(self.nj) self.q_out = np.zeros(self.nj) ret, self.dest, self.q_out = self.generate_dest() self.fin_position = self.dest.p return
def __init__(self): rospy.loginfo("Creating VelocityController class") # Create KDL model with cl.suppress_stdout_stderr(): # Eliminates urdf tag warnings self.robot = URDF.from_parameter_server() # sawyer's URDF # self.kin = KDLKinematics(self.robot, "base", "right_gripper") # kinematics to custom joint frame self.kin = KDLKinematics(self.robot, "base", "right_gripper_tip") self.names = self.kin.get_joint_names() if rospy.has_param('vel_calc'): rospy.delete_param('vel_calc') self.limb = intera_interface.Limb("right") #self._limb = Limb() self.gripper = intera_interface.gripper.Gripper('right') # Grab M0 and Blist from saywer_MR_description.py self.M0 = s.M #Zero config of right_hand self.Blist = s.Blist #6x7 screw axes mx of right arm self.Kp = 50.0 * np.eye(6) self.Ki = 0.0 * np.eye(6) self.Kd = 100.0 * np.eye(6) # add D-gain self.it_count = 0 self.int_err = 0 self.der_err = 0 self.int_anti_windup = 10 self.rate = 100.0 #Hz self.sample_time = 1.0 / self.rate * 2.5 #ms self.USE_FIXED_RATE = False # If true -> dT = 0.01, Else -> function call time difference self.current_time = time.time() self.last_time = self.current_time self.last_error = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # error in cartesian space # Shared variables self.mutex = threading.Lock() self.time_limit = rospy.get_param("~time_limit", TIME_LIMIT) self.damping = rospy.get_param("~damping", DAMPING) self.joint_vel_limit = rospy.get_param("~joint_vel_limit", JOINT_VEL_LIMIT) self.q = np.zeros(7) # Joint angles self.qdot = np.zeros(7) # Joint velocities self.T_goal = np.array(self.kin.forward(self.q)) # Ref se3 self.cur_frame = pm.fromMatrix(self.T_goal) # -> PyKDL frame # robot @ its original pose : position (0.3161, 0.1597, 1.151) , orientation (0.337, 0.621, 0.338, 0.621) self.original_goal = self.T_goal.copy() # robot @ its home pose : print('Verifying initial pose...') print(self.T_goal) self.isCalcOK = False self.isPathPlanned = False self.traj_err_bound = float(1e-2) # in meter self.plan_time = 2.5 # in sec. can this be random variable? self.rand_plan_min = 5.0 self.rand_plan_max = 9.0 # Subscriber self.ee_position = [0.0, 0.0, 0.0] self.ee_orientation = [0.0, 0.0, 0.0, 0.0] self.ee_lin_twist = [0.0, 0.0, 0.0] self.ee_ang_twist = [0.0, 0.0, 0.0] rospy.Subscriber('/demo/target/', Pose, self.ref_poseCB) self.gripper.calibrate() # path planning self.num_wp = int(0) self.cur_wp_idx = int(0) # [0:num_wp - 1] self.traj_list = [None for _ in range(self.num_wp)] self.traj_elapse = 0.0 # in ms self.r = rospy.Rate(self.rate) # robot chain for the ik solver of PyKDL # This constructor parses the URDF loaded in rosparm urdf_param into the # needed KDL structures. self._base = self.robot.get_root() self._tip = "right_gripper_tip" self.sawyer_tree = kdl_tree_from_urdf_model( self.robot) # sawyer's kinematic tree self.sawyer_chain = self.sawyer_tree.getChain(self._base, self._tip) # KDL solvers self.ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self.sawyer_chain) self._num_jnts = 7 self._joint_names = [ 'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6' ] rospy.loginfo('Check the URDF of sawyer') self.print_robot_description() rospy.loginfo('Check the sanity of kinematic chain') self.print_kdl_chain() self.prev_frame = PyKDL.Frame() # initialize as identity frame self.init_frame = PyKDL.Frame( ) # frame of the start pose of the trajectory self._frame1 = PyKDL.Frame() self._frame2 = PyKDL.Frame() self.pose1 = Pose() self.pose2 = Pose() rospy.Subscriber("test", itzy, callback=self.msgggg) self.vr0_T_saw0 = np.identity(4) self.btn_state = 0 self.btn_state_2 = 0 self._gripper = intera_interface.Gripper() rospy.Subscriber('vive_right', Joy, callback=self.grip_open_close) # control loop rospy.Subscriber('/robot/limb/right/endpoint_state', EndpointState, self.endpoint_poseCB) while not rospy.is_shutdown(): # if rospy.has_param('vel_calc'): # if not self.isPathPlanned: # if path is not planned # self.path_planning() # get list of planned waypoints # # self.calc_joint_vel_2() self.calc_joint_vel_3() self.r.sleep()
def __init__(self): filename = '/home/kaiser/WS_ROS/catkin_ws/src/urdf_serp/urdf/urdf_exportado4.urdf' datos_articulaciones = open('datos_articulaciones.pkl', 'wb') robot = urdf_parser_py.urdf.URDF.from_xml_file(filename) (ok, tree) = kdl_parser_py.urdf.treeFromFile(filename) cadena_der_up_down = tree.getChain("base_link", "pie_der_link") cadena_der_down_up = tree.getChain("pie_der_link", "base_link") cadena_izq_up_down = tree.getChain("base_link", "pie_izq_link") cadena_izq_down_up = tree.getChain("pie_izq_link", "base_link") print cadena_der_up_down.getNrOfSegments() fksolver_der_up_down = PyKDL.ChainFkSolverPos_recursive(cadena_der_up_down) fksolver_der_down_up = PyKDL.ChainFkSolverPos_recursive(cadena_der_down_up) fksolver_izq_up_down = PyKDL.ChainFkSolverPos_recursive(cadena_izq_up_down) fksolver_izq_down_up = PyKDL.ChainFkSolverPos_recursive(cadena_izq_down_up) vik_der_up_down = PyKDL.ChainIkSolverVel_pinv(cadena_der_up_down) ik_der_up_down = PyKDL.ChainIkSolverPos_NR(cadena_der_up_down, fksolver_der_up_down, vik_der_up_down) vik_der_down_up = PyKDL.ChainIkSolverVel_pinv(cadena_der_down_up) ik_der_down_up = PyKDL.ChainIkSolverPos_NR(cadena_der_down_up, fksolver_der_down_up, vik_der_down_up) vik_izq_up_down = PyKDL.ChainIkSolverVel_pinv(cadena_izq_up_down) ik_izq_up_down = PyKDL.ChainIkSolverPos_NR(cadena_izq_up_down, fksolver_izq_up_down, vik_izq_up_down) vik_izq_down_up = PyKDL.ChainIkSolverVel_pinv(cadena_izq_down_up) ik_izq_down_up = PyKDL.ChainIkSolverPos_NR(cadena_izq_down_up, fksolver_izq_down_up, vik_izq_down_up) nj_izq = cadena_der_up_down.getNrOfJoints() posicionInicial_der_up_down = PyKDL.JntArray(nj_izq) posicionInicial_der_up_down[0] = 0.3 posicionInicial_der_up_down[1] = -0.3 posicionInicial_der_up_down[2] = 0 posicionInicial_der_up_down[3] = 0.6 posicionInicial_der_up_down[4] = -0.3 posicionInicial_der_up_down[5] = -0.3 nj_izq = cadena_izq_up_down.getNrOfJoints() posicionInicial_izq_up_down = PyKDL.JntArray(nj_izq) posicionInicial_izq_up_down[0] = 0.3 posicionInicial_izq_up_down[1] = -0.3 posicionInicial_izq_up_down[2] = 0.0 posicionInicial_izq_up_down[3] = 0.6 posicionInicial_izq_up_down[4] = -0.3 posicionInicial_izq_up_down[5] = -0.3 nj_izq = cadena_der_down_up.getNrOfJoints() posicionInicial_der_down_up = PyKDL.JntArray(nj_izq) posicionInicial_der_down_up[5] = 0.3 posicionInicial_der_down_up[4] = -0.3 posicionInicial_der_down_up[3] = 0.0 posicionInicial_der_down_up[2] = 0.6 posicionInicial_der_down_up[1] = -0.3 posicionInicial_der_down_up[0] = -0.3 nj_izq = cadena_izq_down_up.getNrOfJoints() posicionInicial_izq_down_up = PyKDL.JntArray(nj_izq) posicionInicial_izq_down_up[5] = 0.3 posicionInicial_izq_down_up[4] = -0.3 posicionInicial_izq_down_up[3] = 0.0 posicionInicial_izq_down_up[2] = 0.6 posicionInicial_izq_down_up[1] = -0.3 posicionInicial_izq_down_up[0] = -0.3 print "Forward kinematics" finalFrame_izq_up_down = PyKDL.Frame() finalFrame_izq_down_up = PyKDL.Frame() finalFrame_der_up_down = PyKDL.Frame() finalFrame_der_down_up = PyKDL.Frame() print "Rotational Matrix of the final Frame: " print finalFrame_izq_up_down.M print "End-effector position: ", finalFrame_izq_up_down.p rospy.init_node('trajectory_demo') # Set to True to move back to the starting configurations reset = rospy.get_param('~reset', False) # Set to False to wait for arm to finish before moving head sync = rospy.get_param('~sync', True) # Which joints define the arm? piernas_joints = ['cilinder_blue1_box1_der_joint', 'cilinder_blue_box1_der_joint', 'cilinder_blue_box2_der_joint', 'cilinder_blue_box4_der_joint', 'cilinder_blue1_box6_der_joint', 'cilinder_blue_box6_der_joint', 'cilinder_blue1_box1_izq_joint', 'cilinder_blue_box1_izq_joint', 'cilinder_blue_box2_izq_joint', 'cilinder_blue_box4_izq_joint', 'cilinder_blue1_box6_izq_joint', 'cilinder_blue_box6_izq_joint' ] piernas_goal = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] #11111111111111111111111111111111111111111111 print "Inverse Kinematics" fksolver_izq_up_down.JntToCart(posicionInicial_izq_up_down, finalFrame_izq_up_down) fksolver_izq_down_up.JntToCart(posicionInicial_izq_down_up, finalFrame_izq_down_up) q_init_izq_up_down = posicionInicial_izq_up_down # initial angles desiredFrame = finalFrame_izq_up_down desiredFrame.p[0] = finalFrame_izq_up_down.p[0] desiredFrame.p[1] = finalFrame_izq_up_down.p[1] desiredFrame.p[2] = finalFrame_izq_up_down.p[2] print "Desired Position: ", desiredFrame.p q_out_izq_up_down = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints()) ik_izq_up_down.CartToJnt(q_init_izq_up_down, desiredFrame, q_out_izq_up_down) print "Output angles in rads: ", q_out_izq_up_down piernas_goal[6] = q_out_izq_up_down[0] piernas_goal[7] = q_out_izq_up_down[1] piernas_goal[8] = q_out_izq_up_down[2] piernas_goal[9] = q_out_izq_up_down[3] piernas_goal[10] = q_out_izq_up_down[4] piernas_goal[11] = q_out_izq_up_down[5] print "Inverse Kinematics" fksolver_der_up_down.JntToCart(posicionInicial_der_up_down, finalFrame_der_up_down) fksolver_der_down_up.JntToCart(posicionInicial_der_down_up, finalFrame_der_down_up) q_init_der_up_down = posicionInicial_der_up_down # initial angles # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1)) desiredFrame = finalFrame_der_up_down desiredFrame.p[0] = finalFrame_der_up_down.p[0] desiredFrame.p[1] = finalFrame_der_up_down.p[1] desiredFrame.p[2] = finalFrame_der_up_down.p[2]+0.02 #0.02 print "Desired Position: ", desiredFrame.p q_out_der_up_down = PyKDL.JntArray(cadena_der_up_down.getNrOfJoints()) ik_der_up_down.CartToJnt(q_init_der_up_down, desiredFrame, q_out_der_up_down) print "Output angles in rads: ", q_out_der_up_down piernas_goal[0] = q_out_der_up_down[0] piernas_goal[1] = q_out_der_up_down[1] piernas_goal[2] = q_out_der_up_down[2] piernas_goal[3] = q_out_der_up_down[3] piernas_goal[4] = q_out_der_up_down[4] piernas_goal[5] = q_out_der_up_down[5] #121212122121212121212121212121212121212121212 piernas_goal12 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] print "Inverse Kinematics" desiredFrame = PyKDL.Frame() fksolver_izq_up_down.JntToCart(q_out_izq_up_down, desiredFrame) fksolver_izq_down_up.JntToCart(posicionInicial_izq_down_up, finalFrame_izq_down_up) q_init_izq_up_down = posicionInicial_izq_up_down # initial angles desiredFrame.p[0] = desiredFrame.p[0] desiredFrame.p[1] = desiredFrame.p[1] desiredFrame.p[2] = desiredFrame.p[2] print "Desired Position: ", desiredFrame.p q_out_izq_up_down = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints()) ik_izq_up_down.CartToJnt(q_init_izq_up_down, desiredFrame, q_out_izq_up_down) print "Output angles in rads: ", q_out_izq_up_down piernas_goal12[6] = q_out_izq_up_down[0] piernas_goal12[7] = q_out_izq_up_down[1] piernas_goal12[8] = q_out_izq_up_down[2] piernas_goal12[9] = q_out_izq_up_down[3] piernas_goal12[10] = q_out_izq_up_down[4] piernas_goal12[11] = q_out_izq_up_down[5] print "Inverse Kinematics" desiredFrame0 = PyKDL.Frame() fksolver_der_up_down.JntToCart(q_out_der_up_down, desiredFrame0) fksolver_der_down_up.JntToCart(posicionInicial_der_down_up, finalFrame_der_down_up) q_init_der_up_down = posicionInicial_der_up_down # initial angles # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1)) desiredFrame0.p[0] = desiredFrame0.p[0] desiredFrame0.p[1] = desiredFrame0.p[1] -0.07 desiredFrame0.p[2] = desiredFrame0.p[2] print "Desired Position: ", desiredFrame0.p q_out_der_up_down = PyKDL.JntArray(cadena_der_up_down.getNrOfJoints()) ik_der_up_down.CartToJnt(q_init_der_up_down, desiredFrame0, q_out_der_up_down) print "Output angles in rads: ", q_out_der_up_down piernas_goal12[0] = q_out_der_up_down[0] piernas_goal12[1] = q_out_der_up_down[1] piernas_goal12[2] = q_out_der_up_down[2] piernas_goal12[3] = q_out_der_up_down[3] piernas_goal12[4] = q_out_der_up_down[4] piernas_goal12[5] = q_out_der_up_down[5] # 222222222222222222222222222222222222222222 piernas_goal2 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] print "Inverse Kinematics" desiredFrame2 = PyKDL.Frame() fksolver_izq_down_up.JntToCart(posicionInicial_izq_down_up, desiredFrame2) q_init_izq_down_up = posicionInicial_izq_down_up # initial angles # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1)) desiredFrame2.p[0] = desiredFrame2.p[0] -0.06 #0.05 desiredFrame2.p[1] = desiredFrame2.p[1] -0.06#0.06 desiredFrame2.p[2] = desiredFrame2.p[2] -0.01 #0.02 print "Desired Position2222aaa: ", desiredFrame2.p #print desiredFrame2.M q_out_izq_down_up = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints()) ik_izq_down_up.CartToJnt(q_init_izq_down_up, desiredFrame2, q_out_izq_down_up) print "Output angles in rads2222: ", q_out_izq_down_up piernas_goal2[6] = q_out_izq_down_up[5]#+0.1 piernas_goal2[7] = q_out_izq_down_up[4]#-0.05 piernas_goal2[8] = q_out_izq_down_up[3] piernas_goal2[9] = q_out_izq_down_up[2] piernas_goal2[10] = q_out_izq_down_up[1] piernas_goal2[11] = q_out_izq_down_up[0] #q_out_izq_down_up[4] -=0.1 print "Inverse Kinematics" q_init_der_down_up = PyKDL.JntArray(6) q_init_der_down_up[0] = q_out_der_up_down[5] # PROBLEMAS CON LA ASIGNACION q_init_der_down_up[1] = q_out_der_up_down[4] q_init_der_down_up[2] = q_out_der_up_down[3] q_init_der_down_up[3] = q_out_der_up_down[2] q_init_der_down_up[4] = q_out_der_up_down[1] q_init_der_down_up[5] = q_out_der_up_down[0]+0.08 fksolver_der_down_up.JntToCart(q_init_der_down_up,finalFrame_der_down_up) # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1)) desiredFrame3 = finalFrame_der_down_up desiredFrame3.p[0] = finalFrame_der_down_up.p[0]- 0.05 desiredFrame3.p[1] = finalFrame_der_down_up.p[1]- 0.04 desiredFrame3.p[2] = finalFrame_der_down_up.p[2]-0.02 print "Desired Position2222der: ", desiredFrame3.p q_out_der_down_up = PyKDL.JntArray(cadena_der_down_up.getNrOfJoints()) ik_der_down_up.CartToJnt(q_init_der_down_up, desiredFrame3, q_out_der_down_up) print "Output angles in rads22222der: ", q_out_der_down_up print "VALOR", q_out_der_up_down[5] piernas_goal2[0] = -0.3 piernas_goal2[1] = -0.3 piernas_goal2[2] = 0 piernas_goal2[3] = 0.6 piernas_goal2[4] = 0.3 piernas_goal2[5] = -0.3 # 333333333333333333333333333333333333333333333333 piernas_goal3 = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] print "Inverse Kinematics" desiredFrame4 = PyKDL.Frame() fksolver_izq_down_up.JntToCart(q_out_izq_down_up,desiredFrame4) # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1)) desiredFrame4.p[0] = desiredFrame4.p[0] desiredFrame4.p[1] = desiredFrame4.p[1] - 0.02 desiredFrame4.p[2] = desiredFrame4.p[2] ik_izq_down_up.CartToJnt(q_out_izq_down_up, desiredFrame4, q_out_izq_down_up) q_init_izq_up_down[0] = q_out_izq_down_up[5] q_init_izq_up_down[1] = q_out_izq_down_up[4] q_init_izq_up_down[2] = q_out_izq_down_up[3] q_init_izq_up_down[3] = q_out_izq_down_up[2] q_init_izq_up_down[4] = q_out_izq_down_up[1] q_init_izq_up_down[5] = q_out_izq_down_up[0] desiredFrame5 = PyKDL.Frame() fksolver_izq_up_down.JntToCart(q_init_izq_up_down,desiredFrame5) desiredFrame5.p[0] = desiredFrame5.p[0] desiredFrame5.p[1] = desiredFrame5.p[1] - 0.1 desiredFrame5.p[2] = desiredFrame5.p[2]+0.01 print "Desired Position: ", desiredFrame5.p q_out_izq_up_down2 = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints()) ik_izq_up_down.CartToJnt(q_init_izq_up_down, desiredFrame5, q_out_izq_up_down2) print "Output angles in rads: ", q_out_izq_up_down2 piernas_goal3[6] = q_out_izq_up_down2[0] piernas_goal3[7] = q_out_izq_up_down2[1] piernas_goal3[8] = q_out_izq_up_down2[2] piernas_goal3[9] = q_out_izq_up_down2[3] piernas_goal3[10] = q_out_izq_up_down2[4]#+0.15 piernas_goal3[11] = q_out_izq_up_down2[5]-0.08 print "Inverse Kinematics" piernas_goal3[0] = -0.3 piernas_goal3[1] = -0.3 piernas_goal3[2] = 0 piernas_goal3[3] = 0.6 piernas_goal3[4] = 0.3 piernas_goal3[5] = -0.3 # 121212122121212121212121212121212121212121212 piernas_goal121 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] print "Inverse Kinematics" desiredFrame6 = PyKDL.Frame() fksolver_izq_up_down.JntToCart(q_out_izq_up_down2, desiredFrame6) fksolver_izq_down_up.JntToCart(posicionInicial_izq_down_up, finalFrame_izq_down_up) q_init_izq_up_down = q_out_izq_up_down2 # initial angles #CUIDADO desiredFrame6.p[0] = desiredFrame6.p[0] desiredFrame6.p[1] = desiredFrame6.p[1]-0.05 desiredFrame6.p[2] = desiredFrame6.p[2]#+0.01 print "Desired Position: ", desiredFrame6.p q_out_izq_up_down = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints()) ik_izq_up_down.CartToJnt(q_init_izq_up_down, desiredFrame6, q_out_izq_up_down) print "Output angles in rads_izq_121: ", q_out_izq_up_down piernas_goal121[6] = q_out_izq_up_down[0] piernas_goal121[7] = q_out_izq_up_down[1] piernas_goal121[8] = q_out_izq_up_down[2] piernas_goal121[9] = q_out_izq_up_down[3] piernas_goal121[10] = q_out_izq_up_down[4] piernas_goal121[11] = q_out_izq_up_down[5] print "Inverse Kinematics" desiredFrame06 = PyKDL.Frame() fksolver_der_up_down.JntToCart(q_out_der_up_down, desiredFrame06) fksolver_der_down_up.JntToCart(posicionInicial_der_down_up, finalFrame_der_down_up) q_init_der_up_down = q_out_der_up_down # initial angles # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1)) desiredFrame06.p[0] = desiredFrame06.p[0] desiredFrame06.p[1] = desiredFrame06.p[1] desiredFrame06.p[2] = desiredFrame06.p[2] print "Desired Position: ", desiredFrame06.p q_out_der_up_down = PyKDL.JntArray(cadena_der_up_down.getNrOfJoints()) ik_der_up_down.CartToJnt(q_init_der_up_down, desiredFrame06, q_out_der_up_down) print "Output angles in rads: ", q_out_der_up_down q_out_der_up_down21 = PyKDL.JntArray(6) q_out_der_up_down21 = [-.3, -.3, 0, .6, .3, -.3 ] piernas_goal121[0] = q_out_der_up_down21[0] piernas_goal121[1] = q_out_der_up_down21[1] piernas_goal121[2] = q_out_der_up_down21[2] piernas_goal121[3] = q_out_der_up_down21[3] piernas_goal121[4] = q_out_der_up_down21[4] piernas_goal121[5] = q_out_der_up_down21[5] # 55555555555555555555555555555555555555555 piernas_goal25 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] print "Inverse Kinematics" desiredFrame7= PyKDL.Frame() q_init_izq_down_up3 = PyKDL.JntArray(6) q_init_izq_down_up3[0] = q_out_izq_up_down[5] * 1 q_init_izq_down_up3[1] = q_out_izq_up_down[4] * 1 q_init_izq_down_up3[2] = q_out_izq_up_down[3] * 1 q_init_izq_down_up3[3] = q_out_izq_up_down[2] * 1 q_init_izq_down_up3[4] = q_out_izq_up_down[1] * 1 q_init_izq_down_up3[5] = q_out_izq_up_down[0] * 1 fksolver_izq_down_up.JntToCart(q_init_izq_down_up3, desiredFrame7) # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1)) desiredFrame7.p[0] = desiredFrame7.p[0] + 0.05#0.03 # PROBAR A PONERLO MAYOR desiredFrame7.p[1] = desiredFrame7.p[1] - 0.06#0.04 desiredFrame7.p[2] = desiredFrame7.p[2] + 0.005 print "Desired Position2222: ", desiredFrame7.p q_out_izq_down_up5 = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints()) ik_izq_down_up.CartToJnt(q_init_izq_down_up3, desiredFrame7, q_out_izq_down_up5) print "Output angles in rads2222AAAAA: ", q_out_izq_down_up5 piernas_goal25[6] = q_out_izq_down_up5[5] piernas_goal25[7] = q_out_izq_down_up5[4] piernas_goal25[8] = q_out_izq_down_up5[3] piernas_goal25[9] = q_out_izq_down_up5[2] piernas_goal25[10] = q_out_izq_down_up5[1]-0.05 piernas_goal25[11] = q_out_izq_down_up5[0]+0.05 print "Inverse Kinematics" q_init_der_down_up31 = PyKDL.JntArray(6) q_init_der_down_up31[0] = q_out_der_up_down21[5] *1 # PROBLEMAS CON LA ASIGNACION q_init_der_down_up31[1] = q_out_der_up_down21[4] *1 q_init_der_down_up31[2] = q_out_der_up_down21[3] *1 q_init_der_down_up31[3] = q_out_der_up_down21[2] *1 q_init_der_down_up31[4] = q_out_der_up_down21[1] *1 q_init_der_down_up31[5] = q_out_der_up_down21[0] *1 desiredFrame7 = PyKDL.Frame() fksolver_der_down_up.JntToCart(q_init_der_down_up31, desiredFrame7) # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1)) desiredFrame7.p[0] = desiredFrame7.p[0] + 0.05 desiredFrame7.p[1] = desiredFrame7.p[1] - 0.06 desiredFrame7.p[2] = desiredFrame7.p[2] - 0.02 print "Desired Position2222der: ", desiredFrame7.p q_out_der_down_up71 = PyKDL.JntArray(cadena_der_down_up.getNrOfJoints()) ik_der_down_up.CartToJnt(q_init_der_down_up31, desiredFrame7, q_out_der_down_up71) print "Output angles in rads22222der: ", q_out_der_down_up71 piernas_goal25[0] = q_out_der_down_up71[5] piernas_goal25[1] = q_out_der_down_up71[4] piernas_goal25[2] = q_out_der_down_up71[3] piernas_goal25[3] = q_out_der_down_up71[2] piernas_goal25[4] = q_out_der_down_up71[1] piernas_goal25[5] = q_out_der_down_up71[0] # 333333333333333333333333333333333333333333333333 piernas_goal341 = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] print "Inverse Kinematics" desiredFrame441 = PyKDL.Frame() fksolver_der_down_up.JntToCart(q_out_der_down_up71, desiredFrame441) # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1)) desiredFrame441.p[0] = desiredFrame441.p[0] desiredFrame441.p[1] = desiredFrame441.p[1] - 0.02 desiredFrame441.p[2] = desiredFrame441.p[2] - 0.015 ik_der_down_up.CartToJnt(q_out_der_down_up71, desiredFrame441, q_out_der_down_up71) q_init_der_up_down[0] = q_out_der_down_up71[5] q_init_der_up_down[1] = q_out_der_down_up71[4] q_init_der_up_down[2] = q_out_der_down_up71[3] q_init_der_up_down[3] = q_out_der_down_up71[2] q_init_der_up_down[4] = q_out_der_down_up71[1] q_init_der_up_down[5] = q_out_der_down_up71[0] desiredFrame541 = PyKDL.Frame() fksolver_der_up_down.JntToCart(q_init_der_up_down, desiredFrame541) desiredFrame541.p[0] = desiredFrame541.p[0] - 0.03 desiredFrame541.p[1] = desiredFrame541.p[1] - 0.1 desiredFrame541.p[2] = desiredFrame541.p[2] - 0.01 #nada print "Desired Position: ", desiredFrame541.p q_out_der_up_down241 = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints()) ik_der_up_down.CartToJnt(q_init_der_up_down, desiredFrame541, q_out_der_up_down241)# con desiredFrame5 va print "Output angles in radsaaaaa: ", q_out_der_up_down241 print "Inverse Kinematics" piernas_goal341[6] = 0.3 piernas_goal341[7] = -0.3 piernas_goal341[8] = 0 piernas_goal341[9] = 0.6 piernas_goal341[10] = -0.3 piernas_goal341[11] = -0.3 piernas_goal341[0] = q_out_der_up_down241[0]#+0.1 piernas_goal341[1] = q_out_der_up_down241[1] piernas_goal341[2] = q_out_der_up_down241[2] piernas_goal341[3] = q_out_der_up_down241[3] piernas_goal341[4] = q_out_der_up_down241[4]#-0.1 piernas_goal341[5] = q_out_der_up_down241[5]#-0.01 pickle.dump(piernas_goal, datos_articulaciones, -1) pickle.dump(piernas_goal12, datos_articulaciones, -1) pickle.dump(piernas_goal2, datos_articulaciones, -1) pickle.dump(piernas_goal3, datos_articulaciones, -1) pickle.dump(piernas_goal121, datos_articulaciones, -1) pickle.dump(piernas_goal25, datos_articulaciones, -1) pickle.dump(piernas_goal341, datos_articulaciones, -1) datos_articulaciones.close() # Connect to the right arm trajectory action server rospy.loginfo('Waiting for right arm trajectory controller...') arm_client = actionlib.SimpleActionClient('/piernas/piernas_controller/follow_joint_trajectory', FollowJointTrajectoryAction) #/piernas/piernas_controller/follow_joint_trajectory arm_client.wait_for_server() rospy.loginfo('...connected.') # Create a single-point arm trajectory with the piernas_goal as the end-point arm_trajectory = JointTrajectory() arm_trajectory.joint_names = piernas_joints arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[0].positions = piernas_goal arm_trajectory.points[0].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[0].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[0].time_from_start = rospy.Duration(2.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[1].positions = piernas_goal12 arm_trajectory.points[1].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[1].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[1].time_from_start = rospy.Duration(4.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[2].positions = piernas_goal2 arm_trajectory.points[2].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[2].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[2].time_from_start = rospy.Duration(6.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[3].positions = piernas_goal3 arm_trajectory.points[3].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[3].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[3].time_from_start = rospy.Duration(8.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[4].positions = piernas_goal121 arm_trajectory.points[4].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[4].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[4].time_from_start = rospy.Duration(10.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[5].positions = piernas_goal25 arm_trajectory.points[5].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[5].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[5].time_from_start = rospy.Duration(12.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[6].positions = piernas_goal341 arm_trajectory.points[6].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[6].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[6].time_from_start = rospy.Duration(14.0) '''arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[7].positions = piernas_goal12 arm_trajectory.points[7].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[7].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[7].time_from_start = rospy.Duration(17.5) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[8].positions = piernas_goal2 arm_trajectory.points[8].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[8].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[8].time_from_start = rospy.Duration(19.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[9].positions = piernas_goal3 arm_trajectory.points[9].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[9].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[9].time_from_start = rospy.Duration(21.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[10].positions = piernas_goal121 arm_trajectory.points[10].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[10].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[10].time_from_start = rospy.Duration(23.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[11].positions = piernas_goal25 arm_trajectory.points[11].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[11].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[11].time_from_start = rospy.Duration(25.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[12].positions = piernas_goal341 arm_trajectory.points[12].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[12].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[12].time_from_start = rospy.Duration(28.0)''' # Send the trajectory to the arm action server rospy.loginfo('Moving the arm to goal position...') # Create an empty trajectory goal piernas_goal = FollowJointTrajectoryGoal() # Set the trajectory component to the goal trajectory created above piernas_goal.trajectory = arm_trajectory # Specify zero tolerance for the execution time piernas_goal.goal_time_tolerance = rospy.Duration(0.01) # Send the goal to the action server arm_client.send_goal(piernas_goal) if not sync: # Wait for up to 5 seconds for the motion to complete arm_client.wait_for_result(rospy.Duration(5.0)) arm_client.wait_for_result() print arm_client.get_result() rospy.loginfo('...done')
def __init__(self): filename = '/home/kaiser/WS_ROS/catkin_ws/src/urdf_serp/urdf/mi_cuadrupedo_exp.urdf' angulo_avance = +0.4 #rad altura_pata = +0.06 #cm avance_x = 0.03 # angulo_avance = 0 # +0.40 #rad # altura_pata = 0 # -0.04 #cm avance_x = 0.11 robot = urdf_parser_py.urdf.URDF.from_xml_file(filename) (ok, tree) = kdl_parser_py.urdf.treeFromFile(filename) cadena_RR = tree.getChain("base_link", "tarsus_RR") cadena_RF = tree.getChain("base_link", "tarsus_RF") cadena_LR = tree.getChain("base_link", "tarsus_LR") cadena_LF = tree.getChain("base_link", "tarsus_LF") print cadena_RR.getNrOfSegments() fksolver_RR = PyKDL.ChainFkSolverPos_recursive(cadena_RR) fksolver_RF = PyKDL.ChainFkSolverPos_recursive(cadena_RF) fksolver_LR = PyKDL.ChainFkSolverPos_recursive(cadena_LR) fksolver_LF = PyKDL.ChainFkSolverPos_recursive(cadena_LF) vik_RR = PyKDL.ChainIkSolverVel_pinv(cadena_RR) ik_RR = PyKDL.ChainIkSolverPos_NR(cadena_RR, fksolver_RR, vik_RR) vik_RF = PyKDL.ChainIkSolverVel_pinv(cadena_RF) ik_RF = PyKDL.ChainIkSolverPos_NR(cadena_RF, fksolver_RF, vik_RF) vik_LR = PyKDL.ChainIkSolverVel_pinv(cadena_LR) ik_LR = PyKDL.ChainIkSolverPos_NR(cadena_LR, fksolver_LR, vik_LR) vik_LF = PyKDL.ChainIkSolverVel_pinv(cadena_LF) ik_LF = PyKDL.ChainIkSolverPos_NR(cadena_LF, fksolver_LF, vik_LF) nj_izq = cadena_RR.getNrOfJoints() posicionInicial_R = PyKDL.JntArray(nj_izq) posicionInicial_R[0] = 0 posicionInicial_R[1] = 0 posicionInicial_R[2] = 0 posicionInicial_R[3] = 0 nj_izq = cadena_LR.getNrOfJoints() posicionInicial_L = PyKDL.JntArray(nj_izq) posicionInicial_L[0] = 0 posicionInicial_L[1] = 0 posicionInicial_L[2] = 0 posicionInicial_L[3] = 0 print "Forward kinematics" finalFrame_R = PyKDL.Frame() finalFrame_L = PyKDL.Frame() rospy.init_node('trajectory_demo') # Set to True to move back to the starting configurations reset = rospy.get_param('~reset', False) # Set to False to wait for arm to finish before moving head sync = rospy.get_param('~sync', True) # Which joints define the arm? piernas_joints_RR = [ 'coxa_joint_RR', 'femur_joint_RR', 'tibia_joint_RR', 'tarsus_joint_RR' ] piernas_joints_RF = [ 'coxa_joint_RF', 'femur_joint_RF', 'tibia_joint_RF', 'tarsus_joint_RF' ] piernas_joints_LR = [ 'coxa_joint_LR', 'femur_joint_LR', 'tibia_joint_LR', 'tarsus_joint_LR' ] piernas_joints_LF = [ 'coxa_joint_LF', 'femur_joint_LF', 'tibia_joint_LF', 'tarsus_joint_LF' ] rr_goal0 = [0.0, 0.0, 0.0, 0.0] rf_goal0 = [0.0, 0.0, 0.0, 0.0] rr_goal1 = [0.0, 0.0, 0.0, 0.0] rf_goal1 = [0.0, 0.0, 0.0, 0.0] lr_goal0 = [0.0, 0.0, 0.0, 0.0] lf_goal0 = [0.0, 0.0, 0.0, 0.0] lr_goal1 = [0.0, 0.0, 0.0, 0.0] lf_goal1 = [0.0, 0.0, 0.0, 0.0] #11111111111111111111111111111111111111111111 print "Inverse Kinematics" fksolver_RR.JntToCart(posicionInicial_R, finalFrame_R) q_init_RR = posicionInicial_R # initial angles desiredFrameRR = finalFrame_R desiredFrameRR.p[0] = finalFrame_R.p[0] + avance_x desiredFrameRR.p[1] = finalFrame_R.p[1] desiredFrameRR.p[2] = finalFrame_R.p[2] + altura_pata print "Desired Position: ", desiredFrameRR.p q_out_RR = PyKDL.JntArray(cadena_RR.getNrOfJoints()) ik_RR.CartToJnt(q_init_RR, desiredFrameRR, q_out_RR) print "Output angles in rads: ", q_out_RR rr_goal0[0] = q_out_RR[0] rr_goal0[1] = q_out_RR[1] #+angulo_avance rr_goal0[2] = q_out_RR[2] rr_goal0[3] = q_out_RR[3] print "Inverse Kinematics" fksolver_LF.JntToCart(posicionInicial_L, finalFrame_L) q_init_LF = posicionInicial_L # initial angles desiredFrameLF = finalFrame_L desiredFrameLF.p[0] = finalFrame_L.p[0] + avance_x desiredFrameLF.p[1] = finalFrame_L.p[1] desiredFrameLF.p[2] = finalFrame_L.p[2] + altura_pata print "Desired Position: ", desiredFrameLF.p q_out_LF = PyKDL.JntArray(cadena_LF.getNrOfJoints()) ik_LF.CartToJnt(q_init_LF, desiredFrameLF, q_out_LF) print "Output angles in rads: ", q_out_LF lf_goal0[0] = q_out_LF[0] lf_goal0[1] = q_out_LF[1] #- angulo_avance lf_goal0[2] = q_out_LF[2] lf_goal0[3] = q_out_LF[3] # 22222222222222222222222222222222222222222222 RR_actual = PyKDL.JntArray(nj_izq) RR_actual[0] = rr_goal0[0] RR_actual[1] = rr_goal0[1] RR_actual[2] = rr_goal0[2] RR_actual[3] = rr_goal0[3] print "Inverse Kinematics" fksolver_RR.JntToCart(RR_actual, finalFrame_R) q_init_RR = RR_actual # initial angles desiredFrameRR = finalFrame_R desiredFrameRR.p[0] = finalFrame_R.p[0] - avance_x desiredFrameRR.p[1] = finalFrame_R.p[1] desiredFrameRR.p[2] = finalFrame_R.p[2] - altura_pata print "Desired Position: ", desiredFrameRR.p q_out_RR = PyKDL.JntArray(cadena_RR.getNrOfJoints()) ik_RR.CartToJnt(q_init_RR, desiredFrameRR, q_out_RR) print "Output angles in rads: ", q_out_RR rr_goal1[0] = q_out_RR[0] rr_goal1[1] = q_out_RR[1] rr_goal1[2] = q_out_RR[2] rr_goal1[3] = q_out_RR[3] LF_actual = PyKDL.JntArray(nj_izq) LF_actual[0] = lf_goal0[0] LF_actual[1] = lf_goal0[1] LF_actual[2] = lf_goal0[2] LF_actual[3] = lf_goal0[3] print "Inverse Kinematics" fksolver_LF.JntToCart(LF_actual, finalFrame_L) q_init_LF = LF_actual # initial angles desiredFrameLF = finalFrame_L desiredFrameLF.p[0] = finalFrame_L.p[0] - avance_x desiredFrameLF.p[1] = finalFrame_L.p[1] desiredFrameLF.p[2] = finalFrame_L.p[2] - altura_pata print "Desired Position: ", desiredFrameLF.p q_out_LF = PyKDL.JntArray(cadena_LF.getNrOfJoints()) ik_LF.CartToJnt(q_init_LF, desiredFrameLF, q_out_LF) print "Output angles in rads: ", q_out_LF lf_goal1[0] = q_out_LF[0] lf_goal1[1] = q_out_LF[1] # - angulo_avance lf_goal1[2] = q_out_LF[2] lf_goal1[3] = q_out_LF[3] # 11111111111111111111111111111111111111111111 print "Inverse Kinematics" fksolver_LR.JntToCart(posicionInicial_L, finalFrame_L) q_init_LR = posicionInicial_L # initial angles desiredFrameLR = finalFrame_L desiredFrameLR.p[0] = finalFrame_L.p[0] #-avance_x desiredFrameLR.p[1] = finalFrame_L.p[1] desiredFrameLR.p[2] = finalFrame_L.p[2] # + altura_pata print "Desired Position: ", desiredFrameLR.p q_out_LR = PyKDL.JntArray(cadena_LR.getNrOfJoints()) ik_LR.CartToJnt(q_init_LR, desiredFrameLR, q_out_LR) print "Output angles in rads: ", q_out_LR lr_goal0[0] = q_out_LR[0] lr_goal0[1] = q_out_LR[1] + angulo_avance lr_goal0[2] = q_out_LR[2] lr_goal0[3] = q_out_LR[3] print "Inverse Kinematics" fksolver_RF.JntToCart(posicionInicial_R, finalFrame_R) q_init_RF = posicionInicial_R # initial angles desiredFrameRF = finalFrame_R desiredFrameRF.p[0] = finalFrame_R.p[0] # -avance_x desiredFrameRF.p[1] = finalFrame_R.p[1] desiredFrameRF.p[2] = finalFrame_R.p[2] #+ altura_pata print "Desired Position: ", desiredFrameRF.p q_out_RF = PyKDL.JntArray(cadena_RF.getNrOfJoints()) ik_RF.CartToJnt(q_init_RF, desiredFrameRF, q_out_RF) print "Output angles in rads: ", q_out_RF rf_goal0[0] = q_out_RF[0] rf_goal0[1] = q_out_RF[1] - angulo_avance rf_goal0[2] = q_out_RF[2] rf_goal0[3] = q_out_RF[3] # 2222222222222222222222222222222222222222222222 LR_actual = PyKDL.JntArray(nj_izq) LR_actual[0] = lr_goal0[0] LR_actual[1] = lr_goal0[1] LR_actual[2] = lr_goal0[2] LR_actual[3] = lr_goal0[3] print "Inverse Kinematics" fksolver_LR.JntToCart(LR_actual, finalFrame_L) q_init_LR = LR_actual # initial angles print "Inverse Kinematics" desiredFrameLR = finalFrame_L desiredFrameLR.p[0] = finalFrame_L.p[0] #+ avance_x desiredFrameLR.p[1] = finalFrame_L.p[1] desiredFrameLR.p[2] = finalFrame_L.p[2] # - altura_pata print "Desired Position: ", desiredFrameLR.p q_out_LR = PyKDL.JntArray(cadena_LR.getNrOfJoints()) ik_LR.CartToJnt(q_init_LR, desiredFrameLR, q_out_LR) print "Output angles in rads: ", q_out_LR lr_goal1[0] = q_out_LR[0] #- angulo_avance lr_goal1[1] = q_out_LR[1] lr_goal1[2] = q_out_LR[2] lr_goal1[3] = q_out_LR[3] + angulo_avance RF_actual = PyKDL.JntArray(nj_izq) RF_actual[0] = rf_goal0[0] RF_actual[1] = rf_goal0[1] RF_actual[2] = rf_goal0[2] RF_actual[3] = rf_goal0[3] print "Inverse Kinematics" fksolver_RF.JntToCart(RF_actual, finalFrame_R) q_init_RF = RF_actual # initial angles desiredFrameRF = finalFrame_R desiredFrameRF.p[0] = finalFrame_R.p[0] # + avance_x desiredFrameRF.p[1] = finalFrame_R.p[1] desiredFrameRF.p[2] = finalFrame_R.p[2] #- altura_pata print "Desired Position: ", desiredFrameRF.p q_out_RF = PyKDL.JntArray(cadena_RF.getNrOfJoints()) ik_RF.CartToJnt(q_init_RF, desiredFrameRF, q_out_RF) print "Output angles in rads: ", q_out_RF rf_goal1[0] = q_out_RF[0] rf_goal1[1] = q_out_RF[1] rf_goal1[2] = q_out_RF[2] rf_goal1[3] = q_out_RF[3] + angulo_avance # Connect to the right arm trajectory action server rospy.loginfo('Waiting for right arm trajectory controller...') rr_client = actionlib.SimpleActionClient( '/cuadrupedo/pata_rr/follow_joint_trajectory', FollowJointTrajectoryAction) rr_client.wait_for_server() rf_client = actionlib.SimpleActionClient( '/cuadrupedo/pata_rf/follow_joint_trajectory', FollowJointTrajectoryAction) rf_client.wait_for_server() lr_client = actionlib.SimpleActionClient( '/cuadrupedo/pata_lr/follow_joint_trajectory', FollowJointTrajectoryAction) lr_client.wait_for_server() lf_client = actionlib.SimpleActionClient( '/cuadrupedo/pata_lf/follow_joint_trajectory', FollowJointTrajectoryAction) lf_client.wait_for_server() rospy.loginfo('...connected.') # Create a single-point arm trajectory with the piernas_goal as the end-point rr_trajectory1 = JointTrajectory() rr_trajectory1.joint_names = piernas_joints_RR rr_trajectory1.points.append(JointTrajectoryPoint()) rr_trajectory1.points[0].positions = rr_goal0 rr_trajectory1.points[0].velocities = [0.0 for i in piernas_joints_RR] rr_trajectory1.points[0].accelerations = [ 0.0 for i in piernas_joints_RR ] rr_trajectory1.points[0].time_from_start = rospy.Duration(0.2) rr_trajectory1.points.append(JointTrajectoryPoint()) rr_trajectory1.points[1].positions = rr_goal1 rr_trajectory1.points[1].velocities = [0.0 for i in piernas_joints_RR] rr_trajectory1.points[1].accelerations = [ 0.0 for i in piernas_joints_RR ] rr_trajectory1.points[1].time_from_start = rospy.Duration(0.4) rr_trajectory1.points.append(JointTrajectoryPoint()) rr_trajectory1.points[2].positions = rf_goal0 rr_trajectory1.points[2].velocities = [0.0 for i in piernas_joints_RR] rr_trajectory1.points[2].accelerations = [ 0.0 for i in piernas_joints_RR ] rr_trajectory1.points[2].time_from_start = rospy.Duration(0.6) rr_trajectory1.points.append(JointTrajectoryPoint()) rr_trajectory1.points[3].positions = rf_goal1 rr_trajectory1.points[3].velocities = [0.0 for i in piernas_joints_RR] rr_trajectory1.points[3].accelerations = [ 0.0 for i in piernas_joints_RR ] rr_trajectory1.points[3].time_from_start = rospy.Duration(0.8) #''' rf_trajectory1 = JointTrajectory() rf_trajectory1.joint_names = piernas_joints_RF rf_trajectory1.points.append(JointTrajectoryPoint()) rf_trajectory1.points[0].positions = rf_goal0 # [0,0,0,0] rf_trajectory1.points[0].velocities = [0.0 for i in piernas_joints_RF] rf_trajectory1.points[0].accelerations = [ 0.0 for i in piernas_joints_RF ] rf_trajectory1.points[0].time_from_start = rospy.Duration(0.2) rf_trajectory1.points.append(JointTrajectoryPoint()) rf_trajectory1.points[1].positions = rf_goal1 # [0,0,0,0] rf_trajectory1.points[1].velocities = [0.0 for i in piernas_joints_RF] rf_trajectory1.points[1].accelerations = [ 0.0 for i in piernas_joints_RF ] rf_trajectory1.points[1].time_from_start = rospy.Duration(0.4) rf_trajectory1.points.append(JointTrajectoryPoint()) rf_trajectory1.points[2].positions = rr_goal0 # [0,0,0,0] rf_trajectory1.points[2].velocities = [0.0 for i in piernas_joints_RF] rf_trajectory1.points[2].accelerations = [ 0.0 for i in piernas_joints_RF ] rf_trajectory1.points[2].time_from_start = rospy.Duration(0.6) rf_trajectory1.points.append(JointTrajectoryPoint()) rf_trajectory1.points[3].positions = rr_goal1 # [0,0,0,0] rf_trajectory1.points[3].velocities = [0.0 for i in piernas_joints_RF] rf_trajectory1.points[3].accelerations = [ 0.0 for i in piernas_joints_RF ] rf_trajectory1.points[3].time_from_start = rospy.Duration(0.8) # ''' lr_trajectory1 = JointTrajectory() lr_trajectory1.joint_names = piernas_joints_LR lr_trajectory1.points.append(JointTrajectoryPoint()) lr_trajectory1.points[0].positions = lr_goal0 #lr_goal0 lr_trajectory1.points[0].velocities = [0.0 for i in piernas_joints_RR] lr_trajectory1.points[0].accelerations = [ 0.0 for i in piernas_joints_RR ] lr_trajectory1.points[0].time_from_start = rospy.Duration(0.2) lr_trajectory1.points.append(JointTrajectoryPoint()) lr_trajectory1.points[1].positions = lr_goal1 lr_trajectory1.points[1].velocities = [0.0 for i in piernas_joints_RR] lr_trajectory1.points[1].accelerations = [ 0.0 for i in piernas_joints_RR ] lr_trajectory1.points[1].time_from_start = rospy.Duration(0.4) lr_trajectory1.points.append(JointTrajectoryPoint()) lr_trajectory1.points[2].positions = lf_goal0 lr_trajectory1.points[2].velocities = [0.0 for i in piernas_joints_RR] lr_trajectory1.points[2].accelerations = [ 0.0 for i in piernas_joints_RR ] lr_trajectory1.points[2].time_from_start = rospy.Duration(0.6) lr_trajectory1.points.append(JointTrajectoryPoint()) lr_trajectory1.points[3].positions = lf_goal1 # lr_goal0 lr_trajectory1.points[3].velocities = [0.0 for i in piernas_joints_RR] lr_trajectory1.points[3].accelerations = [ 0.0 for i in piernas_joints_RR ] lr_trajectory1.points[3].time_from_start = rospy.Duration(0.8) # ''' lf_trajectory1 = JointTrajectory() lf_trajectory1.joint_names = piernas_joints_LF lf_trajectory1.points.append(JointTrajectoryPoint()) lf_trajectory1.points[0].positions = lf_goal0 #lf_goal0 # [0,0,0,0] lf_trajectory1.points[0].velocities = [0.0 for i in piernas_joints_RF] lf_trajectory1.points[0].accelerations = [ 0.0 for i in piernas_joints_RF ] lf_trajectory1.points[0].time_from_start = rospy.Duration(0.2) lf_trajectory1.points.append(JointTrajectoryPoint()) lf_trajectory1.points[1].positions = lf_goal1 # [0,0,0,0] lf_trajectory1.points[1].velocities = [0.0 for i in piernas_joints_RF] lf_trajectory1.points[1].accelerations = [ 0.0 for i in piernas_joints_RF ] lf_trajectory1.points[1].time_from_start = rospy.Duration(0.4) lf_trajectory1.points.append(JointTrajectoryPoint()) lf_trajectory1.points[2].positions = lr_goal0 # [0,0,0,0] lf_trajectory1.points[2].velocities = [0.0 for i in piernas_joints_RF] lf_trajectory1.points[2].accelerations = [ 0.0 for i in piernas_joints_RF ] lf_trajectory1.points[2].time_from_start = rospy.Duration(0.6) lf_trajectory1.points.append(JointTrajectoryPoint()) lf_trajectory1.points[3].positions = lr_goal1 # lf_goal0 # [0,0,0,0] lf_trajectory1.points[3].velocities = [0.0 for i in piernas_joints_RF] lf_trajectory1.points[3].accelerations = [ 0.0 for i in piernas_joints_RF ] lf_trajectory1.points[3].time_from_start = rospy.Duration(0.8) # ''' # Send the trajectory to the arm action server rospy.loginfo('Moving the arm to goal position...') lf_reposo_trajectory1 = JointTrajectory() lf_reposo_trajectory1.joint_names = piernas_joints_LF lf_reposo_trajectory1.points.append(JointTrajectoryPoint()) lf_reposo_trajectory1.points[0].positions = [ 0.0 for i in piernas_joints_RF ] lf_reposo_trajectory1.points[0].velocities = [ 0.0 for i in piernas_joints_RF ] lf_reposo_trajectory1.points[0].accelerations = [ 0.0 for i in piernas_joints_RF ] lf_reposo_trajectory1.points[0].time_from_start = rospy.Duration(0.2) lr_reposo_trajectory1 = JointTrajectory() lr_reposo_trajectory1.joint_names = piernas_joints_LR lr_reposo_trajectory1.points.append(JointTrajectoryPoint()) lr_reposo_trajectory1.points[0].positions = [ 0.0 for i in piernas_joints_RF ] lr_reposo_trajectory1.points[0].velocities = [ 0.0 for i in piernas_joints_RF ] lr_reposo_trajectory1.points[0].accelerations = [ 0.0 for i in piernas_joints_RF ] lr_reposo_trajectory1.points[0].time_from_start = rospy.Duration(0.2) rr_reposo_trajectory1 = JointTrajectory() rr_reposo_trajectory1.joint_names = piernas_joints_RR rr_reposo_trajectory1.points.append(JointTrajectoryPoint()) rr_reposo_trajectory1.points[0].positions = [ 0.0 for i in piernas_joints_RF ] rr_reposo_trajectory1.points[0].velocities = [ 0.0 for i in piernas_joints_RF ] rr_reposo_trajectory1.points[0].accelerations = [ 0.0 for i in piernas_joints_RF ] rr_reposo_trajectory1.points[0].time_from_start = rospy.Duration(0.2) rf_reposo_trajectory1 = JointTrajectory() rf_reposo_trajectory1.joint_names = piernas_joints_RF rf_reposo_trajectory1.points.append(JointTrajectoryPoint()) rf_reposo_trajectory1.points[0].positions = [ 0.0 for i in piernas_joints_RF ] rf_reposo_trajectory1.points[0].velocities = [ 0.0 for i in piernas_joints_RF ] rf_reposo_trajectory1.points[0].accelerations = [ 0.0 for i in piernas_joints_RF ] rf_reposo_trajectory1.points[0].time_from_start = rospy.Duration(0.2) # Create an empty trajectory goal rr_goal = FollowJointTrajectoryGoal() lm_goal = FollowJointTrajectoryGoal() rf_goal = FollowJointTrajectoryGoal() lr_goal = FollowJointTrajectoryGoal() rm_goal = FollowJointTrajectoryGoal() lf_goal = FollowJointTrajectoryGoal() # Set the trajectory component to the goal trajectory created above rr_goal.trajectory = rr_trajectory1 rf_goal.trajectory = rf_trajectory1 lr_goal.trajectory = lr_trajectory1 lf_goal.trajectory = lf_trajectory1 # Specify zero tolerance for the execution time rr_goal.goal_time_tolerance = rospy.Duration(0.0) lm_goal.goal_time_tolerance = rospy.Duration(0.0) rf_goal.goal_time_tolerance = rospy.Duration(0.0) lr_goal.goal_time_tolerance = rospy.Duration(0.0) rm_goal.goal_time_tolerance = rospy.Duration(0.0) lf_goal.goal_time_tolerance = rospy.Duration(0.0) # Send the goal to the action server rr_client.send_goal(rr_goal) rf_client.send_goal(rf_goal) #rr_client.wait_for_result(rospy.Duration(5.0)) print 'Resultado recibido' lr_client.send_goal(lr_goal) lf_client.send_goal(lf_goal) rr_client.wait_for_result() rr_client.send_goal(rr_goal) rf_client.send_goal(rf_goal) # rr_client.wait_for_result(rospy.Duration(5.0)) print 'Resultado recibido' lr_client.send_goal(lr_goal) lf_client.send_goal(lf_goal) rr_client.wait_for_result() rr_client.send_goal(rr_goal) rf_client.send_goal(rf_goal) # rr_client.wait_for_result(rospy.Duration(5.0)) print 'Resultado recibido' lr_client.send_goal(lr_goal) lf_client.send_goal(lf_goal) rr_client.wait_for_result() rr_client.send_goal(rr_goal) rf_client.send_goal(rf_goal) # rr_client.wait_for_result(rospy.Duration(5.0)) print 'Resultado recibido' lr_client.send_goal(lr_goal) lf_client.send_goal(lf_goal) rr_client.wait_for_result() rr_client.send_goal(rr_goal) rf_client.send_goal(rf_goal) # rr_client.wait_for_result(rospy.Duration(5.0)) print 'Resultado recibido' lr_client.send_goal(lr_goal) lf_client.send_goal(lf_goal) rr_client.wait_for_result() rr_client.send_goal(rr_goal) rf_client.send_goal(rf_goal) # rr_client.wait_for_result(rospy.Duration(5.0)) print 'Resultado recibido' lr_client.send_goal(lr_goal) lf_client.send_goal(lf_goal) rr_client.wait_for_result() rr_client.send_goal(rr_goal) rf_client.send_goal(rf_goal) # rr_client.wait_for_result(rospy.Duration(5.0)) print 'Resultado recibido' lr_client.send_goal(lr_goal) lf_client.send_goal(lf_goal) rr_client.wait_for_result() rr_client.send_goal(rr_goal) rf_client.send_goal(rf_goal) # rr_client.wait_for_result(rospy.Duration(5.0)) print 'Resultado recibido' lr_client.send_goal(lr_goal) lf_client.send_goal(lf_goal) rr_client.wait_for_result() rr_client.send_goal(rr_goal) rf_client.send_goal(rf_goal) # rr_client.wait_for_result(rospy.Duration(5.0)) print 'Resultado recibido' lr_client.send_goal(lr_goal) lf_client.send_goal(lf_goal) rr_client.wait_for_result() rr_client.send_goal(rr_goal) rf_client.send_goal(rf_goal) # rr_client.wait_for_result(rospy.Duration(5.0)) print 'Resultado recibido' lr_client.send_goal(lr_goal) lf_client.send_goal(lf_goal) #lr_client.wait_for_result(rospy.Duration(5.0)) #rr_client.send_goal(rr_goal) #lm_client.send_goal(lm_goal) #rf_client.send_goal(rf_goal)''' if not sync: # Wait for up to 5 seconds for the motion to complete rr_client.wait_for_result(rospy.Duration(5.0)) rr_client.wait_for_result() print rr_client.get_result() rr_goal.trajectory = rr_reposo_trajectory1 rf_goal.trajectory = rf_reposo_trajectory1 lr_goal.trajectory = lr_reposo_trajectory1 lf_goal.trajectory = lf_reposo_trajectory1 rr_client.send_goal(rr_goal) rf_client.send_goal(rf_goal) # rr_client.wait_for_result(rospy.Duration(5.0)) print 'Resultado recibido' lr_client.send_goal(lr_goal) lf_client.send_goal(lf_goal) rr_client.wait_for_result() rospy.loginfo('...done')
def __init__(self, name, base, ee_link, namespace=None, arm_name=None, compute_fk_for_all=False): # Joint states self._joint_angles = dict() self._joint_velocity = dict() self._joint_effort = dict() # Finger chain name self._name = name # Namespace if None in [namespace, arm_name]: ns = [ item for item in rospy.get_namespace().split('/') if len(item) > 0 ] if len(ns) != 2: rospy.ROSException( 'The controller must be called inside the namespace the manipulator namespace' ) self._namespace = ns[0] self._arm_name = ns[1] else: self._namespace = namespace self._arm_name = arm_name if self._namespace[0] != '/': self._namespace = '/' + self._namespace if self._namespace[-1] != '/': self._namespace += '/' # True if it has to compute the forward kinematics for all frames self._compute_fk_for_all = compute_fk_for_all # List of frames for each link self._frames = dict() # Load robot description (complete robot, including vehicle, if # available) self._robot_description = URDF.from_parameter_server( key=self._namespace + 'robot_description') # KDL tree of the whole structure ok, self._kdl_tree = treeFromUrdfModel(self._robot_description) # Base link self._base_link = base # Tip link self._tip_link = ee_link # Read the complete link name, with robot's namespace, if existent for link in self._robot_description.links: if self._arm_name not in link.name: continue linkname = link.name.split('/')[-1] if self._base_link == linkname: self._base_link = link.name if self._tip_link == linkname: self._tip_link = link.name # Get arm chain self._chain = self._kdl_tree.getChain(self._base_link, self._tip_link) # Partial tree from base to each link self._chain_part = dict() for link in self._robot_description.links: if link.name in self.link_names: self._chain_part[link.name] = self._kdl_tree.getChain( self._base_link, link.name) # Get joint names from the chain # Joint names self._joint_names = list() for idx in xrange(self._chain.getNrOfSegments()): joint = self._chain.getSegment(idx).getJoint() # Not considering fixed joints if joint.getType() == 0: name = joint.getName() self._joint_names.append(name) self._joint_angles[name] = 0.0 self._joint_velocity[name] = 0.0 self._joint_effort[name] = 0.0 # Jacobian solvers self._jac_kdl = PyKDL.ChainJntToJacSolver(self._chain) # Jacobian solver dictionary for partial trees self._jac_kdl_part = dict() for link in self._robot_description.links: if link.name in self.link_names: self._jac_kdl_part[link.name] = PyKDL.ChainJntToJacSolver( self._chain_part[link.name]) # Forward position kinematics solvers self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._chain) # Forward velocity kinematics solvers self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._chain) # Inverse kinematic solvers self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._chain) self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._chain, self._fk_p_kdl, self._ik_v_kdl, 100, 1e-6)
def create_solvers(self, ch): fk = kdl.ChainFkSolverPos_recursive(ch) ik_v = kdl.ChainIkSolverVel_pinv(ch) ik_p = kdl.ChainIkSolverPos_NR(ch, fk, ik_v) jac = kdl.ChainJntToJacSolver(ch) return fk, ik_v, ik_p, jac