def __init__(self, base_link=None, end_link=None): self._robot = kdl_parser_py.urdf.urdf.URDF.from_parameter_server( 'robot_description') (ok, self._kdl_tree) = kdl_parser_py.urdf.treeFromUrdfModel(self._robot) self._base_link = self._robot.get_root() self._tip_link = end_link self._tip_frame = PyKDL.Frame() self._arm_chain = self._kdl_tree.getChain(self._base_link, self._tip_link) #self.joint_names = [joint.name for joint in self._robot.joints if joint.type!='fixed'] self.joint_names = [ self._arm_chain.getSegment(i).getJoint().getName() for i in range(self._arm_chain.getNrOfSegments()) ] self._num_jnts = len(self.joint_names) # Store joint information for future use self.get_joint_information() # 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, urdf_relative_path='../../assets/urdf/sawyer_arm_intera.urdf'): ''' Uses inverse kinematics in order to go from end-effector cartesian velocity commands to joint velocity commands :param urdf_relative_path: The path to the URDF file of the robot, relative to the current directory ''' self.num_joints = 7 # Get the URDF urdf_path = os.path.join(os.path.dirname(os.path.realpath(__file__)), urdf_relative_path) self.urdf_model = URDF.from_xml_string(open(urdf_path).read()) # Set the base link, and the tip link. I think that these are just strings, defined somewhere in the URDF. The name 'right_hand' is how the gripper is described on the real Sawyer, but it may have a different name in your own URDF. self.base_link = self.urdf_model.get_root() self.tip_link = "right_hand" # # Create a KDL tree from the URDF model. This is an object defining the kinematics of the entire robot. self.kdl_tree = treeFromUrdfModel(self.urdf_model) # Create a KDL chain from the tree. This is one specific chain within the overall kinematics model. self.arm_chain = self.kdl_tree[1].getChain(self.base_link, self.tip_link) # Create a solver which will be used to compute the forward kinematics self.forward_kinematics_solver = PyKDL.ChainFkSolverPos_recursive( self.arm_chain) # Create a solver which will be used to compute the Jacobian self.jacobian_solver = PyKDL.ChainJntToJacSolver(self.arm_chain) #Velocity inverse kinematics solver self.IK_v_solver = PyKDL.ChainIkSolverVel_pinv(self.arm_chain) # Create a solver to retrieve the joint angles form the eef position self.IK_solver = PyKDL.ChainIkSolverPos_NR( self.arm_chain, self.forward_kinematics_solver, self.IK_v_solver)
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): self.num_joints = 7 # Get the URDF urdf_path = os.path.join(os.path.dirname(os.path.realpath(__file__)), '../assets/urdf/sawyer_intera.urdf') self.urdf_model = URDF.from_xml_string(open(urdf_path).read()) # Set the base link, and the tip link. self.base_link = self.urdf_model.get_root() self.tip_link = "right_hand" # # Create a KDL tree from the URDF model. This is an object defining the kinematics of the entire robot. self.kdl_tree = treeFromUrdfModel(self.urdf_model) # Create a KDL chain from the tree. This is one specific chain within the overall kinematics model. self.arm_chain = self.kdl_tree[1].getChain(self.base_link, self.tip_link) # Create a solver which will be used to compute the forward kinematics self.forward_kinematics_solver = PyKDL.ChainFkSolverPos_recursive( self.arm_chain) # Create a solver which will be used to compute the Jacobian self.jacobian_solver = PyKDL.ChainJntToJacSolver(self.arm_chain) #Velocity inverse kinematics solver self.IK_v_solver = PyKDL.ChainIkSolverVel_pinv(self.arm_chain) # Create a solver to retrieve the joint angles form the eef position self.IK_solver = PyKDL.ChainIkSolverPos_NR( self.arm_chain, self.forward_kinematics_solver, self.IK_v_solver)
def __init__(self, robot, ee_link=None): rospack = rospkg.RosPack() pykdl_dir = rospack.get_path('ur_pykdl') TREE_PATH = pykdl_dir + '/urdf/' + robot + '.urdf' self._ur = URDF.from_xml_file(TREE_PATH) self._kdl_tree = kdl_tree_from_urdf_model(self._ur) self._base_link = BASE_LINK ee_link = EE_LINK if ee_link is None else ee_link self._tip_link = ee_link self._tip_frame = PyKDL.Frame() self._arm_chain = self._kdl_tree.getChain(self._base_link, self._tip_link) # UR Interface Limb Instances self._joint_names = JOINT_ORDER 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, limb): self._kuka = URDF.from_parameter_server(key='robot_description') self._kdl_tree = kdl_tree_from_urdf_model(self._kuka) self._base_link = self._kuka.get_root() # self._tip_link = limb + '_gripper' self._tip_link = 'tool0' self._tip_frame = PyKDL.Frame() self._arm_chain = self._kdl_tree.getChain(self._base_link, self._tip_link) # Kuka Interface Limb Instances # self._limb_interface = baxter_interface.Limb(limb) # self._joint_names = self._limb_interface.joint_names() self._joint_names = [ 'joint_a1', 'joint_a2', 'joint_a3', 'joint_a4', 'joint_a5', 'joint_a6' ] 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, 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 __init__(self): self.num_joints = 7 # Get the URDF (you will need to write your own "load_urdf()" function) urdf_path = os.path.join(assets_root, './kdl/panda_urdf/panda_arm_hand.urdf') # urdf_path= os.path.join(robosuite.models.assets_root, "bullet_data/sawyer_description/urdf/sawyer_intera.urdf") self.urdf_model = URDF.from_xml_string( open(urdf_path).read().encode( 'ascii')) # encode to ascii solve the error #self.kdl_tree = treeFromFile(urdf_path) # Set the base link, and the tip link. I think that these are just strings, defined somewhere in the URDF. The name 'right_hand' is how the gripper is described on the real Sawyer, but it may have a different name in your own URDF. self.base_link = self.urdf_model.get_root() self.tip_link = "panda_hand" # hand of panda robot, see panda_arm_hand.urdf # # Create a KDL tree from the URDF model. This is an object defining the kinematics of the entire robot. self.kdl_tree = treeFromUrdfModel(self.urdf_model) # Create a KDL chain from the tree. This is one specific chain within the overall kinematics model. self.arm_chain = self.kdl_tree[1].getChain(self.base_link, self.tip_link) # Create a solver which will be used to compute the forward kinematics self.forward_kinematics_solver = PyKDL.ChainFkSolverPos_recursive( self.arm_chain) # Create a solver which will be used to compute the Jacobian self.jacobian_solver = PyKDL.ChainJntToJacSolver(self.arm_chain) #Velocity inverse kinematics solver self.IK_v_solver = PyKDL.ChainIkSolverVel_pinv(self.arm_chain) # Create a solver to retrieve the joint angles form the eef position self.IK_solver = PyKDL.ChainIkSolverPos_NR( self.arm_chain, self.forward_kinematics_solver, self.IK_v_solver)
def __init__(self, limb='right'): 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() # RUDI: LETS GET RID OF INTERA #self._tip_link = limb + '_hand' self._tip_link = 'right_gripper_tip' self._tip_frame = PyKDL.Frame() self._arm_chain = self._kdl_tree.getChain(self._base_link, self._tip_link) # RUDI: LETS GET RID OF INTERA # Sawyer Interface Limb Instances # self._limb_interface = intera_interface.Limb(limb) #self._joint_names = self._limb_interface.joint_names() self._joint_names = ["right_j0", "right_j1", "right_j2", "right_j3", "right_j4", "right_j5", "right_j6"] 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, limb): self._pr2 = URDF.from_parameter_server(key='robot_description') self._kdl_tree = kdl_tree_from_urdf_model(self._pr2) self._base_link = self._pr2.get_root() # self._tip_link = limb + '_gripper' self._tip_link = limb + '_wrist_roll_link' self._tip_frame = PyKDL.Frame() self._arm_chain = self._kdl_tree.getChain(self._base_link, self._tip_link) # Baxter Interface Limb Instances # self._limb_interface = baxter_interface.Limb(limb) # self._joint_names = self._limb_interface.joint_names() self._joint_names = [ limb + '_shoulder_pan_joint', limb + '_shoulder_lift_joint', limb + '_upper_arm_roll_joint', limb + '_elbow_flex_joint', limb + '_forearm_roll_joint', limb + '_wrist_flex_joint', limb + '_wrist_roll_joint' ] 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, 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() print "BASE LINK:", self._base_link 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(0.0, 0.0, -9.81))
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 __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 _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 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 __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 __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 configure(self, tf_base_link_name, tf_end_link_name): self.tf_base_link_name = tf_base_link_name self.tf_end_link_name = tf_end_link_name #Variables holding the joint information self.robot = URDF.from_parameter_server() self.tree = kdl_tree_from_urdf_model(self.robot) self.chain = self.tree.getChain(self.tf_base_link_name, self.tf_end_link_name) self.kdl_kin = KDLKinematics(self.robot, self.tf_base_link_name, self.tf_end_link_name) self.arm_joint_names = self.kdl_kin.get_joint_names() self.jnt_pos = kdl.JntArray(self.chain.getNrOfJoints()) self.fk_solver = kdl.ChainFkSolverPos_recursive(self.chain) self.ikv_solver = kdl.ChainIkSolverVel_wdls(self.chain) self.ikv_solver.setLambda(0.0001) #self.ikv_solver.setWeightTS([[1,0,0,0,0,0],[0,1,0,0,0,0],[0,0,0,0,0,0],[0,0,0,1,0,0],[0,0,0,0,1,0],[0,0,0,0,0,1]]) self.ik_solver = kdl.ChainIkSolverPos_NR(self.chain, self.fk_solver, self.ikv_solver)
def __init__(self): pi4 = 0.7853 filename = '/home/kaiser/WS_ROS/catkin_ws/src/urdf_serp/urdf/mi_robot_xacro4_cambio.urdf' 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 posicionInicial_der_up_down[1] = 0.3 posicionInicial_der_up_down[2] = -0.3 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 posicionInicial_izq_up_down[1] = 0.3 posicionInicial_izq_up_down[2] = -0.3 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 posicionInicial_der_down_up[4] = 0.3 posicionInicial_der_down_up[3] = -0.3 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 posicionInicial_izq_down_up[4] = 0.3 posicionInicial_izq_down_up[3] = -0.3 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_blue_box1_der_joint', 'cilinder_blue1_box2_der_joint','cilinder_blue_box2_der_joint', 'cilinder_blue_box4_der_joint', 'cilinder_blue1_box6_der_joint','cilinder_blue_box6_der_joint', 'cilinder_blue_box1_izq_joint', 'cilinder_blue1_box2_izq_joint','cilinder_blue_box2_izq_joint', 'cilinder_blue_box4_izq_joint', 'cilinder_blue1_box6_izq_joint','cilinder_blue_box6_izq_joint'] piernas_goal0 = [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 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_goal0[6] = q_out_izq_up_down[0] piernas_goal0[7] = q_out_izq_up_down[1] piernas_goal0[8] = q_out_izq_up_down[2] piernas_goal0[9] = q_out_izq_up_down[3] piernas_goal0[10] = q_out_izq_up_down[4] piernas_goal0[11] = q_out_izq_up_down[5] piernas_goal0[0] = q_out_der_up_down[0] piernas_goal0[1] = q_out_der_up_down[1] piernas_goal0[2] = q_out_der_up_down[2] piernas_goal0[3] = q_out_der_up_down[3] piernas_goal0[4] = q_out_der_up_down[4] piernas_goal0[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] #q_out_izq_up_down[0] -= pi4 piernas_goal12[6] = q_out_izq_up_down[0]-2*pi4 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] piernas_goal12[0] = 0 piernas_goal12[1] = 0 piernas_goal12[2] = -0.7 piernas_goal12[3] = 1.4 piernas_goal12[4] = 0 piernas_goal12[5] = -0.7 ######################################################### 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] piernas_goal2[6] = q_out_izq_up_down[0]-2*pi4 piernas_goal2[7] = q_out_izq_up_down[1]-0.3-0.3 piernas_goal2[8] = q_out_izq_up_down[2]-0.3 piernas_goal2[9] = q_out_izq_up_down[3]+0.6 piernas_goal2[10] = q_out_izq_up_down[4]+0.3+0.3 piernas_goal2[11] = q_out_izq_up_down[5] -0.3 piernas_goal2[0] = 0#-pi4 piernas_goal2[1] = -0.25 piernas_goal2[2] = -0.3 piernas_goal2[3] = 0.6 piernas_goal2[4] = 0.25 piernas_goal2[5] = -0.3 ################################################## piernas_goal3 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] piernas_goal3[6] = q_out_izq_up_down[0] +0.0 piernas_goal3[7] = q_out_izq_up_down[1] - 0.3 piernas_goal3[8] = q_out_izq_up_down[2] - 0.3 piernas_goal3[9] = q_out_izq_up_down[3] + 0.6 piernas_goal3[10] = q_out_izq_up_down[4] + 0.3 piernas_goal3[11] = q_out_izq_up_down[5] - 0.3 piernas_goal3[0] = -2*pi4 piernas_goal3[1] = -0.25 piernas_goal3[2] = -0.3 piernas_goal3[3] = 0.6 piernas_goal3[4] = 0.25 piernas_goal3[5] = -0.3 ################################################## piernas_goal4 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] piernas_goal4[6] = q_out_izq_up_down[0] +0.0 piernas_goal4[7] = q_out_izq_up_down[1] - 0.3 +0.3 piernas_goal4[8] = q_out_izq_up_down[2] - 0.3 +0.3 piernas_goal4[9] = q_out_izq_up_down[3] + 0.6 -0.6 piernas_goal4[10] = q_out_izq_up_down[4] + 0.3 -0.3 piernas_goal4[11] = q_out_izq_up_down[5] - 0.3 +0.3 piernas_goal4[0] = -2*pi4 piernas_goal4[1] = -0.25 piernas_goal4[2] = -0.7 piernas_goal4[3] = 1.4 piernas_goal4[4] = 0.25 piernas_goal4[5] = -0.7 # 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_goal0 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(3.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(6.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(9.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(12.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[4].positions = piernas_goal4 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(15.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[5].positions = piernas_goal12 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(18.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[6].positions = piernas_goal2 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(21.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[7].positions = piernas_goal3 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(24) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[8].positions = piernas_goal4 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(27) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[9].positions = piernas_goal12 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(30.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[10].positions = piernas_goal2 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(33.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[11].positions = piernas_goal3 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(36.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[12].positions = piernas_goal4 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(39.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[13].positions = piernas_goal12 arm_trajectory.points[13].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[13].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[13].time_from_start = rospy.Duration(42.0) '''arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[14].positions = piernas_goal4 arm_trajectory.points[14].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[14].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[14].time_from_start = rospy.Duration(32.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/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/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' robot = urdf_parser_py.urdf.URDF.from_xml_file(filename) (ok, tree) = kdl_parser_py.urdf.treeFromFile(filename) chain = tree.getChain("base_link", "link_6") print chain.getNrOfSegments() solverCinematicaDirecta = PyKDL.ChainFkSolverPos_recursive(chain) nj_izq = chain.getNrOfJoints() posicionInicial = PyKDL.JntArray(nj_izq) posicionInicial[0] = 0 posicionInicial[1] = 0 posicionInicial[2] = 0 posicionInicial[3] = 0 posicionInicial[4] = 0 posicionInicial[5] = 0 print "Forward kinematics" finalFrame = PyKDL.Frame() solverCinematicaDirecta.JntToCart(posicionInicial, finalFrame) print "Rotational Matrix of the final Frame: " print finalFrame.M print "End-effector position: ", finalFrame.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? arm_joints = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'] if reset: # Set the arm back to the resting position arm_goal = [0, 0, 0, 0, 0, 0] else: # Set a goal configuration for the arm arm_goal = [-0.3, 0.5, -1.0, 1.8, 0.3, 0.6] print "Inverse Kinematics" q_init = posicionInicial # initial angles vik = PyKDL.ChainIkSolverVel_pinv(chain) ik = PyKDL.ChainIkSolverPos_NR(chain, solverCinematicaDirecta, vik) #desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1)) desiredFrame = finalFrame desiredFrame.p[0] = finalFrame.p[0] desiredFrame.p[1] = finalFrame.p[1] desiredFrame.p[2] = finalFrame.p[2]-0.25 print "Desired Position: ", desiredFrame.p q_out = PyKDL.JntArray(6) ik.CartToJnt(q_init, desiredFrame, q_out) print "Output angles in rads: ", q_out arm_goal[0] = q_out[0] arm_goal[1] = q_out[1] arm_goal[2] = q_out[2] arm_goal[3] = q_out[3] arm_goal[4] = q_out[4] arm_goal[5] = q_out[5] # Connect to the right arm trajectory action server rospy.loginfo('Waiting for right arm trajectory controller...') pub = rospy.Publisher('joint_path_command',JointTrajectory,queue_size=10) #arm_client = actionlib.SimpleActionClient('joint_trajectory_action', FollowJointTrajectoryAction) #arm_client.wait_for_server() rospy.loginfo('...connected.') # Create a single-point arm trajectory with the arm_goal as the end-point arm_trajectory = JointTrajectory() arm_trajectory.joint_names = arm_joints arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[0].positions = arm_goal arm_trajectory.points[0].velocities = [0.0 for i in arm_joints] arm_trajectory.points[0].accelerations = [0.0 for i in arm_joints] arm_trajectory.points[0].time_from_start = rospy.Duration(2.0) # Send the trajectory to the arm action server rospy.loginfo('Moving the arm to goal position...') # Create an empty trajectory goal arm_goal = FollowJointTrajectoryGoal() # Set the trajectory component to the goal trajectory created above arm_goal.trajectory = arm_trajectory # Specify zero tolerance for the execution time arm_goal.goal_time_tolerance = rospy.Duration(0.0) # Send the goal to the action server #arm_client.send_goal(arm_goal) pub.publish(arm_trajectory) while not rospy.is_shutdown(): pub.publish(arm_trajectory) rospy.sleep(rospy.Duration(0.5)) #if not sync: # Wait for up to 5 seconds for the motion to complete #arm_client.wait_for_result(rospy.Duration(5.0)) 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
from urdf_parser_py.urdf import URDF from pykdl_utils.kdl_parser import kdl_tree_from_urdf_model robot = URDF.from_xml_file("/home/d/catkin_ws/src/robot_description/armc_description/urdf/armc_description.urdf") tree = kdl_tree_from_urdf_model(robot) print tree.getNrOfSegments() chain = tree.getChain("base_link", "sensor_link") print chain.getNrOfSegments() print chain.getNrOfJoints() #正运动学 fk = PyKDL.ChainFkSolverPos_recursive(chain) pos = PyKDL.Frame() q = PyKDL.JntArray(7) for i in range(7): q[i] = 0 q[0] = 1 fk_flag = fk.JntToCart(q, pos) print "fk_flag", fk_flag print "pos", pos #逆运动学 ik_v = PyKDL.ChainIkSolverVel_pinv(chain) ik = PyKDL.ChainIkSolverPos_NR(chain, fk, ik_v, maxiter=100, eps=math.pow(10, -9)) qq = PyKDL.JntArray(7) qq_k = PyKDL.JntArray(7) ik.CartToJnt(qq_k, pos, qq) print "qq:", qq
def __init__(self): filename = '/home/kaiser/WS_ROS/catkin_ws/src/urdf_serp/urdf/urdf_exportado3.urdf' 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 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 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 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() fksolver_izq_up_down.JntToCart(posicionInicial_izq_up_down, finalFrame_izq_up_down) 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' ] if reset: # Set the arm back to the resting position arm_goal = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] else: # Set a goal configuration for the arm arm_goal = [0.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] print "Inverse Kinematics" q_init = posicionInicial_izq_up_down # initial angles #desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1)) 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 = PyKDL.JntArray(6) ik_izq_up_down.CartToJnt(q_init, desiredFrame, q_out) print "Output angles in rads: ", q_out #arm_goal[0] = q_out[0] #arm_goal[1] = q_out[1] #arm_goal[2] = q_out[2] #arm_goal[3] = q_out[3] #arm_goal[4] = q_out[4] #arm_goal[5] = q_out[5] # 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) arm_client.wait_for_server() rospy.loginfo('...connected.') # Create a single-point arm trajectory with the arm_goal as the end-point arm_trajectory = JointTrajectory() arm_trajectory.joint_names = piernas_joints arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[0].positions = arm_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) # Send the trajectory to the arm action server rospy.loginfo('Moving the arm to goal position...') # Create an empty trajectory goal arm_goal = FollowJointTrajectoryGoal() # Set the trajectory component to the goal trajectory created above arm_goal.trajectory = arm_trajectory # Specify zero tolerance for the execution time arm_goal.goal_time_tolerance = rospy.Duration(0.0) # Send the goal to the action server arm_client.send_goal(arm_goal) if not sync: # Wait for up to 5 seconds for the motion to complete arm_client.wait_for_result(rospy.Duration(5.0)) rospy.loginfo('...done')
import time from std_msgs.msg import Float32MultiArray from geometry_msgs.msg import Twist, Point import kdl_parser_py.urdf import PyKDL as kdl import tf servo = serial.Serial("/dev/ttyAMA0", 115200) time.sleep(0.5) filename = "../urdf/simple_arm.urdf" (ok, tree) = kdl_parser_py.urdf.treeFromFile(filename) chain = tree.getChain("base", "link7") t = kdl.Tree(tree) fksolverpos = kdl.ChainFkSolverPos_recursive(chain) iksolvervel = kdl.ChainIkSolverVel_pinv(chain) iksolverpos = kdl.ChainIkSolverPos_NR(chain, fksolverpos, iksolvervel) class xero(object): def __init__(self): self._sub = rospy.Subscriber("cntl", Twist, self._callback) self.msg = Float32MultiArray() self.cmd = Twist() self.last_cmd = Twist() self.R = Point() self.L = Point() def _callback(self, cmd): # print (cmd.angular.z, self.last_cmd.angular.z) if cmd.angular.z != self.last_cmd.angular.z: