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')
def __init__(self): filename = '/home/kaiser/WS_ROS/catkin_ws/src/urdf_serp/urdf/mi_hexapodo_exp.urdf' angulo_avance = +0.20 #rad altura_pata = +0.03 #cm # angulo_avance = 0 # +0.40 #rad # altura_pata = 0 # -0.04 #cm 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_RM = tree.getChain("base_link", "tarsus_RM") cadena_RF = tree.getChain("base_link", "tarsus_RF") cadena_LR = tree.getChain("base_link", "tarsus_LR") cadena_LM = tree.getChain("base_link", "tarsus_LM") cadena_LF = tree.getChain("base_link", "tarsus_LF") print cadena_RR.getNrOfSegments() fksolver_RR = PyKDL.ChainFkSolverPos_recursive(cadena_RR) fksolver_RM= PyKDL.ChainFkSolverPos_recursive(cadena_RM) fksolver_RF = PyKDL.ChainFkSolverPos_recursive(cadena_RF) fksolver_LR = PyKDL.ChainFkSolverPos_recursive(cadena_LR) fksolver_LM = PyKDL.ChainFkSolverPos_recursive(cadena_LM) 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_RM = PyKDL.ChainIkSolverVel_pinv(cadena_RM) ik_RM = PyKDL.ChainIkSolverPos_NR(cadena_RM, fksolver_RM, vik_RM) 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_LM = PyKDL.ChainIkSolverVel_pinv(cadena_LM) ik_LM = PyKDL.ChainIkSolverPos_NR(cadena_LM, fksolver_LM, vik_LM) 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_LM = ['coxa_joint_LM', 'femur_joint_LM','tibia_joint_LM', 'tarsus_joint_LM'] 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_RM = ['coxa_joint_RM', 'femur_joint_RM', 'tibia_joint_RM', 'tarsus_joint_RM'] 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] lm_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] lm_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] rm_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] rm_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] 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]+angulo_avance rr_goal0[1] = q_out_RR[1] rr_goal0[2] = q_out_RR[2] rr_goal0[3] = q_out_RR[3] print "Inverse Kinematics" fksolver_LM.JntToCart(posicionInicial_L, finalFrame_L) q_init_LM = posicionInicial_L # initial angles desiredFrameLM = finalFrame_L desiredFrameLM.p[0] = finalFrame_L.p[0] desiredFrameLM.p[1] = finalFrame_L.p[1] desiredFrameLM.p[2] = finalFrame_L.p[2] +altura_pata print "Desired Position: ", desiredFrameLM.p q_out_LM = PyKDL.JntArray(cadena_LM.getNrOfJoints()) ik_LM.CartToJnt(q_init_LM, desiredFrameLM, q_out_LM) print "Output angles in rads: ", q_out_LM lm_goal0[0] = q_out_LM[0]-angulo_avance lm_goal0[1] = q_out_LM[1] lm_goal0[2] = q_out_LM[2] lm_goal0[3] = q_out_LM[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] 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]+angulo_avance rf_goal0[1] = q_out_RF[1] rf_goal0[2] = q_out_RF[2] rf_goal0[3] = q_out_RF[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] 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] rr_goal2 = rr_goal1[:] rr_goal2[0] = -angulo_avance LM_actual = PyKDL.JntArray(nj_izq) LM_actual[0] = lm_goal0[0] LM_actual[1] = lm_goal0[1] LM_actual[2] = lm_goal0[2] LM_actual[3] = lm_goal0[3] print "Inverse Kinematics" fksolver_LM.JntToCart(LM_actual, finalFrame_L) q_init_LM = LM_actual # initial angles desiredFrameLM = finalFrame_L desiredFrameLM.p[0] = finalFrame_L.p[0] desiredFrameLM.p[1] = finalFrame_L.p[1] desiredFrameLM.p[2] = finalFrame_L.p[2] - altura_pata print "Desired Position: ", desiredFrameLM.p q_out_LM = PyKDL.JntArray(cadena_LM.getNrOfJoints()) ik_LM.CartToJnt(q_init_LM, desiredFrameLM, q_out_LM) print "Output angles in rads: ", q_out_LM lm_goal1[0] = q_out_LM[0] lm_goal1[1] = q_out_LM[1] lm_goal1[2] = q_out_LM[2] lm_goal1[3] = q_out_LM[3] lm_goal2 = lm_goal1[:] lm_goal2[0] = 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] 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] rf_goal2 = rf_goal1[:] rf_goal2[0] = -angulo_avance # 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] 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] + angulo_avance lr_goal0[1] = q_out_LR[1] lr_goal0[2] = q_out_LR[2] lr_goal0[3] = q_out_LR[3] print "Inverse Kinematics" fksolver_RM.JntToCart(posicionInicial_R, finalFrame_R) q_init_RM = posicionInicial_R # initial angles desiredFrameRM = finalFrame_R desiredFrameRM.p[0] = finalFrame_R.p[0] desiredFrameRM.p[1] = finalFrame_R.p[1] desiredFrameRM.p[2] = finalFrame_R.p[2] # + altura_pata print "Desired Position: ", desiredFrameRM.p q_out_RM = PyKDL.JntArray(cadena_RM.getNrOfJoints()) ik_RM.CartToJnt(q_init_RM, desiredFrameRM, q_out_RM) print "Output angles in rads: ", q_out_RM rm_goal0[0] = q_out_RM[0] - angulo_avance rm_goal0[1] = q_out_RM[1] rm_goal0[2] = q_out_RM[2] rm_goal0[3] = q_out_RM[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] 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] + angulo_avance lf_goal0[1] = q_out_LF[1] lf_goal0[2] = q_out_LF[2] lf_goal0[3] = q_out_LF[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] 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] RM_actual = PyKDL.JntArray(nj_izq) RM_actual[0] = rm_goal0[0] RM_actual[1] = rm_goal0[1] RM_actual[2] = rm_goal0[2] RM_actual[3] = rm_goal0[3] print "Inverse Kinematics" fksolver_RM.JntToCart(RM_actual, finalFrame_R) q_init_RM = RM_actual # initial angles desiredFrameRM = finalFrame_R desiredFrameRM.p[0] = finalFrame_R.p[0] desiredFrameRM.p[1] = finalFrame_R.p[1] desiredFrameRM.p[2] = finalFrame_R.p[2] + altura_pata print "Desired Position: ", desiredFrameRM.p q_out_RM = PyKDL.JntArray(cadena_RM.getNrOfJoints()) ik_RM.CartToJnt(q_init_RM, desiredFrameRM, q_out_RM) print "Output angles in rads: ", q_out_RM rm_goal1[0] = q_out_RM[0] #- angulo_avance rm_goal1[1] = q_out_RM[1] rm_goal1[2] = q_out_RM[2] rm_goal1[3] = q_out_RM[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] 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] #- angulo_avance lf_goal1[1] = q_out_LF[1] lf_goal1[2] = q_out_LF[2] lf_goal1[3] = q_out_LF[3] # Connect to the right arm trajectory action server rospy.loginfo('Waiting for right arm trajectory controller...') rr_client = actionlib.SimpleActionClient('/hexapodo/pata_rr/follow_joint_trajectory', FollowJointTrajectoryAction) rr_client.wait_for_server() lm_client = actionlib.SimpleActionClient('/hexapodo/pata_lm/follow_joint_trajectory', FollowJointTrajectoryAction) lm_client.wait_for_server() rf_client = actionlib.SimpleActionClient('/hexapodo/pata_rf/follow_joint_trajectory', FollowJointTrajectoryAction) rf_client.wait_for_server() lr_client = actionlib.SimpleActionClient('/hexapodo/pata_lr/follow_joint_trajectory', FollowJointTrajectoryAction) lr_client.wait_for_server() rm_client = actionlib.SimpleActionClient('/hexapodo/pata_rm/follow_joint_trajectory', FollowJointTrajectoryAction) rm_client.wait_for_server() lf_client = actionlib.SimpleActionClient('/hexapodo/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.25) 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.5) rr_trajectory1.points.append(JointTrajectoryPoint()) rr_trajectory1.points[2].positions = rr_goal2 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.75) rr_trajectory1.points.append(JointTrajectoryPoint()) rr_trajectory1.points[3].positions = lr_goal0 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(1.0) #''' lm_trajectory1 = JointTrajectory() lm_trajectory1.joint_names = piernas_joints_LM lm_trajectory1.points.append(JointTrajectoryPoint()) lm_trajectory1.points[0].positions = lm_goal0#[0,0,0,0] lm_trajectory1.points[0].velocities = [0.0 for i in piernas_joints_LM] lm_trajectory1.points[0].accelerations = [0.0 for i in piernas_joints_LM] lm_trajectory1.points[0].time_from_start = rospy.Duration(0.25) lm_trajectory1.points.append(JointTrajectoryPoint()) lm_trajectory1.points[1].positions = lm_goal1 # [0,0,0,0] lm_trajectory1.points[1].velocities = [0.0 for i in piernas_joints_LM] lm_trajectory1.points[1].accelerations = [0.0 for i in piernas_joints_LM] lm_trajectory1.points[1].time_from_start = rospy.Duration(0.5) lm_trajectory1.points.append(JointTrajectoryPoint()) lm_trajectory1.points[2].positions = lm_goal2 # [0,0,0,0] lm_trajectory1.points[2].velocities = [0.0 for i in piernas_joints_LM] lm_trajectory1.points[2].accelerations = [0.0 for i in piernas_joints_LM] lm_trajectory1.points[2].time_from_start = rospy.Duration(0.75) lm_trajectory1.points.append(JointTrajectoryPoint()) lm_trajectory1.points[3].positions = rm_goal0 # [0,0,0,0] lm_trajectory1.points[3].velocities = [0.0 for i in piernas_joints_LM] lm_trajectory1.points[3].accelerations = [0.0 for i in piernas_joints_LM] lm_trajectory1.points[3].time_from_start = rospy.Duration(1.0) # ''' 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.25) 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.5) rf_trajectory1.points.append(JointTrajectoryPoint()) rf_trajectory1.points[2].positions = rf_goal2 # [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.75) rf_trajectory1.points.append(JointTrajectoryPoint()) rf_trajectory1.points[3].positions = lf_goal0 # [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(1.0) # ''' 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.25) 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.5) lr_trajectory1.points.append(JointTrajectoryPoint()) lr_trajectory1.points[2].positions = rr_goal2 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.75) lr_trajectory1.points.append(JointTrajectoryPoint()) lr_trajectory1.points[3].positions = rr_goal0 # 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(1.0) # ''' rm_trajectory1 = JointTrajectory() rm_trajectory1.joint_names = piernas_joints_RM rm_trajectory1.points.append(JointTrajectoryPoint()) rm_trajectory1.points[0].positions = rm_goal0#rm_goal0 # [0,0,0,0] rm_trajectory1.points[0].velocities = [0.0 for i in piernas_joints_LM] rm_trajectory1.points[0].accelerations = [0.0 for i in piernas_joints_LM] rm_trajectory1.points[0].time_from_start = rospy.Duration(0.25) rm_trajectory1.points.append(JointTrajectoryPoint()) rm_trajectory1.points[1].positions = rm_goal1 # [0,0,0,0] rm_trajectory1.points[1].velocities = [0.0 for i in piernas_joints_LM] rm_trajectory1.points[1].accelerations = [0.0 for i in piernas_joints_LM] rm_trajectory1.points[1].time_from_start = rospy.Duration(0.5) rm_trajectory1.points.append(JointTrajectoryPoint()) rm_trajectory1.points[2].positions = lm_goal2 # [0,0,0,0] rm_trajectory1.points[2].velocities = [0.0 for i in piernas_joints_LM] rm_trajectory1.points[2].accelerations = [0.0 for i in piernas_joints_LM] rm_trajectory1.points[2].time_from_start = rospy.Duration(0.75) rm_trajectory1.points.append(JointTrajectoryPoint()) rm_trajectory1.points[3].positions = lm_goal0 # rm_goal0 # [0,0,0,0] rm_trajectory1.points[3].velocities = [0.0 for i in piernas_joints_LM] rm_trajectory1.points[3].accelerations = [0.0 for i in piernas_joints_LM] rm_trajectory1.points[3].time_from_start = rospy.Duration(1.0) # ''' 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.25) 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.5) lf_trajectory1.points.append(JointTrajectoryPoint()) lf_trajectory1.points[2].positions = rf_goal2 # [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.75) lf_trajectory1.points.append(JointTrajectoryPoint()) lf_trajectory1.points[3].positions = rf_goal0 # 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(1.0) # ''' # Send the trajectory to the arm action server rospy.loginfo('Moving the arm to goal position...') # 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 lm_goal.trajectory = lm_trajectory1 rf_goal.trajectory = rf_trajectory1 lr_goal.trajectory = lr_trajectory1 rm_goal.trajectory = rm_trajectory1 lf_goal.trajectory = lf_trajectory1 # Specify zero tolerance for the execution time rr_goal.goal_time_tolerance = rospy.Duration(0.01) lm_goal.goal_time_tolerance = rospy.Duration(0.01) rf_goal.goal_time_tolerance = rospy.Duration(0.01) lr_goal.goal_time_tolerance = rospy.Duration(0.01) rm_goal.goal_time_tolerance = rospy.Duration(0.01) lf_goal.goal_time_tolerance = rospy.Duration(0.01) # Send the goal to the action server rr_client.send_goal(rr_goal) lm_client.send_goal(lm_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) rm_client.send_goal(rm_goal) lf_client.send_goal(lf_goal) rr_client.wait_for_result() rr_client.send_goal(rr_goal) lm_client.send_goal(lm_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) rm_client.send_goal(rm_goal) lf_client.send_goal(lf_goal) rr_client.wait_for_result() rr_client.send_goal(rr_goal) lm_client.send_goal(lm_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) rm_client.send_goal(rm_goal) lf_client.send_goal(lf_goal) rr_client.wait_for_result() rr_client.send_goal(rr_goal) lm_client.send_goal(lm_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) rm_client.send_goal(rm_goal) lf_client.send_goal(lf_goal) rr_client.wait_for_result() rr_client.send_goal(rr_goal) lm_client.send_goal(lm_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) rm_client.send_goal(rm_goal) lf_client.send_goal(lf_goal) rr_client.wait_for_result() rr_client.send_goal(rr_goal) lm_client.send_goal(lm_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) rm_client.send_goal(rm_goal) lf_client.send_goal(lf_goal) rr_client.wait_for_result() rr_client.send_goal(rr_goal) lm_client.send_goal(lm_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) rm_client.send_goal(rm_goal) lf_client.send_goal(lf_goal) rr_client.wait_for_result() rr_client.send_goal(rr_goal) lm_client.send_goal(lm_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) rm_client.send_goal(rm_goal) lf_client.send_goal(lf_goal) rr_client.wait_for_result() rr_client.send_goal(rr_goal) lm_client.send_goal(lm_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) rm_client.send_goal(rm_goal) lf_client.send_goal(lf_goal) rr_client.wait_for_result() rr_client.send_goal(rr_goal) lm_client.send_goal(lm_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) rm_client.send_goal(rm_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() rospy.loginfo('...done')
def __init__(self): 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[0] = 0 posicionInicial_der_down_up[1] = 0.3 posicionInicial_der_down_up[2] = -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[0] = 0 posicionInicial_izq_down_up[1] = 0.3 posicionInicial_izq_down_up[2] = -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 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] 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[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 ] 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 piernas_goal2[1] = 0.3 piernas_goal2[2] = -0.3 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 # 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 = [0.0 for i in piernas_joints] 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(1.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 make_segment(joint): jt = PyKDL.Joint(joint['joint'], joint['origin'], joint['RotAxis'], PyKDL.Joint.RotAxis) seg = PyKDL.Segment(joint['segment'], jt, PyKDL.Frame(joint['origin'])) return seg chain = PyKDL.Chain() for joint in joints: chain.addSegment(make_segment(joint)) fk_kdl = PyKDL.ChainFkSolverPos_recursive(chain) ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(chain) ik_p_kdl = PyKDL.ChainIkSolverPos_NR_JL(chain, q_lower, q_upper, fk_kdl, ik_v_kdl) def fk(q): frame = PyKDL.Frame() fk_kdl.JntToCart(q, frame) return frame def ik(frame, q_guess): q_res = PyKDL.JntArray(6) ik_p_kdl.CartToJnt(q_guess, frame, q_res) return q_res
def callback(data): kdl_chain =PyKDL.Chain() Frame = PyKDL.Frame(); i = 0 d=0 th=0 for joint in dh_file: name = joint['name'] last_d = d last_th = th a = joint["a"] d = joint["d"] al = joint["al"] th = joint["th"] if name == 'i3' and get_parameters('dlugosc1'): a = rospy.get_param("/dlugosc1") if name == 'hand' and get_parameters('dlugosc2'): a = rospy.get_param("/dlugosc2") #forego first iteration if i==0: i = i+1 continue kdl_chain.addSegment(PyKDL.Segment(PyKDL.Joint(PyKDL.Joint.RotZ), Frame.DH(a, al, last_d, last_th))) i = i+1 jointDisplacement = PyKDL.JntArray(kdl_chain.getNrOfJoints()) #joint displacements including restrictions jointDisplacement[0] = data.position[0] jointDisplacement[1] = data.position[1] jointDisplacement[2] = data.position[2] if(data.position[0] < restrictions[0]['backward']): jointDisplacement[0] = restrictions[0]['backward'] rospy.logerr("[KDL] Przekroczono ograniczenie dolne stawu o numerze: 1") elif(data.position[0] > restrictions[0]['forward']): jointDisplacement[0] = restrictions[0]['forward'] rospy.logerr("[KDL] Przekroczono ograniczenie gorne stawu o numerze: 1") if(data.position[1] < restrictions[1]['backward']): jointDisplacement[1] = restrictions[1]['backward'] rospy.logerr("[KDL] Przekroczono ograniczenie dolne stawu o numerze: 2") elif(data.position[1] > restrictions[1]['forward']): jointDisplacement[1] = restrictions[1]['forward'] rospy.logerr("[KDL] Przekroczono ograniczenie gorne stawu o numerze: 2") if(data.position[2] < restrictions[2]['backward']): jointDisplacement[2] = restrictions[2]['backward'] rospy.logerr("[KDL] Przekroczono ograniczenie dolne stawu o numerze: 3") elif(data.position[2] > restrictions[2]['forward']): jointDisplacement[2] = restrictions[2]['forward'] rospy.logerr("[KDL] Przekroczono ograniczenie gorne stawu o numerze: 3") f_k_solver = PyKDL.ChainFkSolverPos_recursive(kdl_chain) frame = PyKDL.Frame() f_k_solver.JntToCart(jointDisplacement, frame) quatr = frame.M.GetQuaternion() kdl_pose = PoseStamped() kdl_pose.header.frame_id = 'base_link' kdl_pose.header.stamp = rospy.Time.now() kdl_pose.pose.position.x = frame.p[0] kdl_pose.pose.position.y = frame.p[1] kdl_pose.pose.position.z = frame.p[2] kdl_pose.pose.orientation.x = quatr[0] kdl_pose.pose.orientation.y = quatr[1] kdl_pose.pose.orientation.z = quatr[2] kdl_pose.pose.orientation.w = quatr[3] pub.publish(kdl_pose)
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.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) if jnt.safety_controller is not None: self.joint_safety_lower.append( jnt.safety_controller.soft_lower_limit) #.lower) self.joint_safety_upper.append( jnt.safety_controller.soft_upper_limit) #.upper) elif jnt.limit is not None: self.joint_safety_lower.append(jnt.limit.lower) self.joint_safety_upper.append(jnt.limit.upper) else: self.joint_safety_lower.append(None) self.joint_safety_upper.append(None) self.joint_types.append(jnt.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, _chain): kdlChain = kdl.Chain() body = _chain.getBaseBody() # Joint Limits self.q_min = kdl.JntArray(len(_chain.getJoints())) self.q_max = kdl.JntArray(len(_chain.getJoints())) old_pos = kdl.Vector(0, 0, 0) index = 0 while (len(body.child_joints) != 0): # Last link # Error if this robot is not a serial robot. if len(body.child_joints) > 1: raise InvalidChainError() # Get Joint Object of type Solver.Joint joint = _chain.getJoint(body.child_joints[0]) # From Joint (Location of joint on parent body) trans = kdl.Vector(float(joint.parent_pivot['x']), float(joint.parent_pivot['y']), float(joint.parent_pivot['z'])) # From Joint (Location of joint on child body) trans2 = kdl.Vector(float(joint.child_pivot['x']), float(joint.child_pivot['y']), float(joint.child_pivot['z'])) frame = kdl.Frame(trans + old_pos) old_pos = trans2 kdlJoint = kdl.Joint() # Default Joint Axis if joint.parent_axis['x']: kdlJoint = kdl.Joint(kdl.Joint.RotX, scale=joint.parent_axis['x']) elif joint.parent_axis['y']: kdlJoint = kdl.Joint(kdl.Joint.RotY, scale=joint.parent_axis['y']) elif joint.parent_axis['z']: kdlJoint = kdl.Joint(kdl.Joint.RotZ, scale=joint.parent_axis['z']) # Add to KDL kdlChain.addSegment(kdl.Segment(kdlJoint, frame)) # Joint limits self.q_min[index] = joint.joint_limits['low'] self.q_max[index] = joint.joint_limits['high'] body = _chain.getBody(body.children[0]) index += 1 self.kdlChain = kdlChain # Initialize Solvers self.FKSolverPos = kdl.ChainFkSolverPos_recursive(self.kdlChain) # self.IKVelSolver = kdl.ChainIkSolverVel_pinv_givens(self.kdlChain) # self.IKPosSolver = kdl.ChainIkSolverPos_NR_JL(self.kdlChain, self.q_min, self.q_max, # self.FKSolverPos, self.IKVelSolver) self.IKPosSolver = kdl.ChainIkSolverPos_LMA(self.kdlChain)
def solveIK(targetFrame): ok, tree = parser.treeFromFile(rospkg.RosPack().get_path('rviz_animator') + "/models/robocam.xml") chain = tree.getChain('base', 'link_camera') plotter = Plotter() # 1. Solve for J4, J5_initial, J6 # First, convert quaternion orientation to XZY order Euler angles targetQuat = targetFrame.M.GetQuaternion() # Get quaternion from KDL frame (x, y, z, w) pitch, yaw, roll = tf.transformations.euler_from_quaternion(targetQuat, axes='rxzy') pitch_deg, yaw_deg, roll_deg = math.degrees(pitch), math.degrees(yaw), math.degrees(roll) J4_origin_orientation = posemath.toMsg(chain.getSegment(2).getFrameToTip()).orientation J4_offset = euler_from_quaternion([J4_origin_orientation.x, J4_origin_orientation.y, J4_origin_orientation.z, J4_origin_orientation.w])[0] J4_raw, J5_initial, J6 = pitch, yaw, roll J4 = J4_raw - J4_offset # 1. Complete above print("J4: {} J5_init: {} J6: {}".format(J4, J5_initial, J6)) chainAngles = PyKDL.JntArray(8) chainAngles[5], chainAngles[6], chainAngles[7] = J4, J5_initial, J6 chainFK = PyKDL.ChainFkSolverPos_recursive(chain) purpleFrame = PyKDL.Frame() brownFrame = PyKDL.Frame() purpleSuccess = chainFK.JntToCart(chainAngles, purpleFrame) # print("Purple success {}".format(purpleSuccess)) print("Target Orientation:\n{}".format(targetFrame.M)) print("Result Orientation:\n{}".format(purpleFrame.M)) brownSuccess = chainFK.JntToCart(chainAngles, brownFrame, segmentNr=7) # 2. Determine position of orange point # First, construct KDL chain of the 3 links involved in J4-J6 cameraOffsetChain = tree.getChain('link_pitch', 'link_camera') cameraJointAngles = PyKDL.JntArray(2) cameraJointAngles[0], cameraJointAngles[1] = J5_initial, J6 cameraOffsetChainFK = PyKDL.ChainFkSolverPos_recursive(cameraOffsetChain) cameraFrame = PyKDL.Frame() success = cameraOffsetChainFK.JntToCart(cameraJointAngles, cameraFrame) # print("FK Status: {}".format(success)) # print("Camera Frame: {}".format(cameraFrame)) # print("End Effector Joint Angles: {}".format([J4, J5_initial, J6])) orangePoint = targetFrame.p - (purpleFrame.p - brownFrame.p) plotter.addVector(targetFrame.p, "pink") plotter.addVector(orangePoint, "orange") plotter.addVector(purpleFrame.p, "purple") plotter.addVector(brownFrame.p, "brown") # print("Target Frame Position: {}".format(targetFrame.p)) # print("Camera Frame Position: {}".format(cameraFrame.p)) # print("Offset: {}".format(targetFrame.p - cameraFrame.p)) # 2. Complete: # 3. Convert orange point to cylindrical coordinates orange_X, orange_Y, orange_Z = orangePoint orange_R = math.sqrt(orange_X ** 2 + orange_Y ** 2) orange_Theta = math.atan2(orange_Y, orange_X) # Theta measured from global positive X axis # 3. Complete: (above) print("Orange R: {} Theta: {}".format(orange_R, math.degrees(orange_Theta))) # 4. Solve for J2 and J3 in the idealized R-Z plane targetVectorOrig = PyKDL.Vector(0, orange_R, orange_Z) plotter.addVector(targetVectorOrig, "targetRZOrig") # First, remove the fixed offsets from the wrist, elbow, and shoulder pieces wristEndFrame = PyKDL.Frame() wristStartFrame = PyKDL.Frame() elbowEndFrame = PyKDL.Frame() elbowStartFrame = PyKDL.Frame() shoulderEndFrame = PyKDL.Frame() shoulderStartFrame = PyKDL.Frame() chainFK.JntToCart(chainAngles, wristEndFrame, segmentNr=7) chainFK.JntToCart(chainAngles, wristStartFrame, segmentNr=5) chainFK.JntToCart(chainAngles, elbowEndFrame, segmentNr=4) chainFK.JntToCart(chainAngles, elbowStartFrame, segmentNr=3) chainFK.JntToCart(chainAngles, shoulderEndFrame, segmentNr=2) chainFK.JntToCart(chainAngles, shoulderStartFrame, segmentNr=0) plotter.addVector(wristEndFrame.p, "wristEndFrame") plotter.addVector(wristStartFrame.p, "wristStartFrame") plotter.addVector(elbowEndFrame.p, "elbowEndFrame") plotter.addVector(elbowStartFrame.p, "elbowStartFrame") plotter.addVector(shoulderEndFrame.p, "shoulderEndFrame") plotter.addVector(shoulderStartFrame.p, "shoulderStartFrame") wristOffset = wristEndFrame.p - wristStartFrame.p elbowOffset = elbowEndFrame.p - elbowStartFrame.p shoulderOffset = shoulderEndFrame.p - shoulderStartFrame.p targetVector = targetVectorOrig - wristOffset - elbowOffset - shoulderOffset plotter.addVector(targetVector, "targetRZ") # The following steps use the same labels as the classic 2D planar IK derivation a1, a2 = (shoulderEndFrame.p - elbowStartFrame.p).Norm(), (elbowEndFrame.p - wristStartFrame.p).Norm() _, ik_x, ik_y = targetVector q2_a = math.acos((ik_x ** 2 + ik_y ** 2 - a1 ** 2 - a2 ** 2) / (2 * a1 * a2)) q1_a = math.atan2(ik_y, ik_x) - math.atan2(a2 * math.sin(-q2_a), a1 + a2 * math.cos(-q2_a)) q2_b = -1 * math.acos((ik_x ** 2 + ik_y ** 2 - a1 ** 2 - a2 ** 2) / (2 * a1 * a2)) q1_b = math.atan2(ik_y, ik_x) + math.atan2(a2 * math.sin(q2_b), a1 + a2 * math.cos(q2_b)) # Choose 'better' set of q1_ab, q2_ab q1, q2 = q1_a, q2_a # TODO(JS): Is this always the better one? J2_initial = q1 J2_offset = math.radians(90) # J2's zero position is vertical, not horizontal J2 = J2_initial - J2_offset # Since we have a parallel link, the angle for J3 is not simply q2. Instead, use transversal J3_initial = q1 - q2 J3_offset = elbowStartFrame.M.GetRPY()[0] # J3's zero position is below horizontal J3 = J3_initial - J3_offset # 4. Complete (above) # print("J2: {} J3: {}".format(J2, J3)) # 5. Use the Theta from cylindrical coordinates as the J1 angle, and update J5 accordingly J1 = orange_Theta - math.radians(90) J5 = J5_initial - (orange_Theta - math.radians(90)) # 5. Complete (above) # print("J1: {} J5: {}".format(J1, J5)) jointAngles = [J1, J2, J3, J4, J5, J6] jointNames = ['joint_base_rot', 'joint_rot_1', 'joint_f1_2', 'joint_f2_pitch', 'joint_pitch_yaw', 'joint_yaw_roll'] print("\n\nFinal joint angles in radians: {}\n\n".format(jointAngles)) in_bounds = True for angle, name in zip(jointAngles, jointNames): limit = robot_urdf.joint_map[name].limit if angle < limit.lower or angle > limit.upper: in_bounds = False print("Joint {} out of bounds with angle {} not between {} and {}".format(name, angle, limit.lower, limit.upper)) print("All in bounds: {}".format(in_bounds)) solvedJoints = PyKDL.JntArray(8) solvedJoints[0], solvedJoints[1], solvedJoints[3], solvedJoints[5], solvedJoints[6], solvedJoints[7] = jointAngles solvedJoints[2], solvedJoints[4] = -1 * solvedJoints[1], -1 * solvedJoints[3] producedFrame = PyKDL.Frame() for i in range(chain.getNrOfSegments()): rc = chainFK.JntToCart(solvedJoints, producedFrame, segmentNr=i) plotter.addVector(producedFrame.p, "fk_produced_{}".format(i)) # print("Result: {}".format(rc)) # print("Output position: {}\nExpected position: {}".format(producedFrame.p, targetFrame.p)) # print("Output orientation: {}\nExpected orientation: {}".format(producedFrame.M, targetFrame.M)) # 6. (optional) Sanity check on solution: # sanityTest(BASE_TO_BASE_YAW, BASE_YAW_TO_BOTTOM_4B, targetFrame, cameraFrame, cameraOffsetChain, jointAngles) # 7. Create JointState message for return ret = JointState() ret.name = ['joint_base_rot', 'joint_rot_1', 'joint_f1_2', 'joint_f2_pitch', 'joint_pitch_yaw', 'joint_yaw_roll'] ret.header.stamp = rospy.Time.now() ret.position = jointAngles plotter.addGoal(ret) # plotter.publish() return in_bounds, ret
frm5 = kdl.Frame(kdl.Rotation.RotX(np.pi / 2), kdl.Vector(0, 0, 0)) frm6 = kdl.Frame(kdl.Rotation.RotX(0), kdl.Vector(-168, 0, 0)) frms.append(frm1) frms.append(frm2) frms.append(frm3) frms.append(frm4) frms.append(frm5) frms.append(frm6) rbt = kdl.Chain() # 建立机器人对象 link = [] for i in range(6): link.append(kdl.Segment(jnts[i], frms[i])) rbt.addSegment(link[i]) fk = kdl.ChainFkSolverPos_recursive(rbt) p = kdl.Frame() q = kdl.JntArray(6) q[0] = 0 q[1] = 0 q[2] = 0 q[3] = 0 q[4] = 0 q[5] = 0 fk.JntToCart(q, p) print(p)
def __init__(self, freq_control=100, margin_workspace=0.05): # Load robot urdf_model = URDF.from_parameter_server() fetch = kdl_tree_from_urdf_model(urdf_model) fetch_arm = fetch.getChain(BASE_LINK, END_LINK) self.dof = fetch_arm.getNrOfJoints() self.kdl_pos = PyKDL.ChainFkSolverPos_recursive(fetch_arm) self.kdl_jac = PyKDL.ChainJntToJacSolver(fetch_arm) self.kdl_dyn = PyKDL.ChainDynParam(fetch_arm, PyKDL.Vector(0, 0, -9.81)) self.kdl_q = PyKDL.JntArray(self.dof) self.kdl_A = PyKDL.JntSpaceInertiaMatrix(self.dof) self.kdl_J = PyKDL.Jacobian(self.dof) self.kdl_x = PyKDL.Frame() # self.kdl_G = PyKDL.JntArray(self.dof) # self.G = np.zeros((self.dof,)) # Initialize robot values self.lock = threading.Lock() self.thread_q = np.zeros((self.dof, )) self.thread_dq = np.zeros((self.dof, )) self.thread_tau = np.zeros((self.dof, )) self.q = np.zeros((self.dof, )) self.dq = np.zeros((self.dof, )) self.tau = np.zeros((self.dof, )) self.x = np.zeros((3, )) self.quat = np.array([0., 0., 0., 1.]) self.lim_norm_x = self.compute_workspace() - margin_workspace self.A = np.zeros((self.dof, self.dof)) self.A_inv = np.zeros((self.dof, self.dof)) self.J = np.zeros((6, self.dof)) self.q_init = np.array([ 0., -np.pi / 4., 0., np.pi / 4, 0., np.pi / 2, 0., ]) # Face down self.q_tuck = np.array([1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]) # Storage position self.q_stow = np.array([1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]) self.q_intermediate_stow = np.array( [0.7, -0.3, 0.0, -0.3, 0.0, -0.57, 0.0]) self.x_des = np.array([0.8, 0., 0.35]) # q_init self.x_init = np.array([0.8, 0., 0.35]) # q_init # self.quat_des = np.array([-0.707, 0., 0.707, 0.]) # Face up self.quat_des = np.array([0., 0.707, 0., 0.707]) # Face down self.state = "INITIALIZE" print("Switching to state: " + self.state) self.t_start = time.time() self.freq_control = freq_control # Initialize pub and sub self.pub = rospy.Publisher("arm_controller/joint_torque/command", Float64MultiArray, queue_size=1) sub = rospy.Subscriber( "joint_states", JointState, lambda joint_states: self.read_joint_sensors(joint_states)) # Initialize ROS rospy.init_node("joint_space_controller")
def callback(data): kdl_chain = kdl.Chain() kdl_frame = kdl.Frame() frame0 = kdl_frame.DH(0, 0, 0, 0) joint0 = kdl.Joint(kdl.Joint.None) kdl_chain.addSegment(kdl.Segment(joint0, frame0)) a, d, alfa, theta = dh['i2'] frame1 = kdl_frame.DH(a, alfa, d, theta) joint1 = kdl.Joint(kdl.Joint.RotZ) kdl_chain.addSegment(kdl.Segment(joint1, frame1)) a, d, alfa, theta = dh['i1'] frame2 = kdl_frame.DH(a, alfa, d, theta) joint2 = kdl.Joint(kdl.Joint.RotZ) kdl_chain.addSegment(kdl.Segment(joint2, frame2)) a, d, alfa, theta = dh['i3'] frame3 = kdl_frame.DH(a, alfa, d, theta) joint3 = kdl.Joint(kdl.Joint.TransZ) kdl_chain.addSegment(kdl.Segment(joint3, frame3)) joint_angles = kdl.JntArray(kdl_chain.getNrOfJoints()) joint_angles[0] = data.position[0] joint_angles[1] = data.position[1] joint_angles[2] = -data.position[2] kdl_chain_t = kdl.ChainFkSolverPos_recursive(kdl_chain) frame_t = kdl.Frame() kdl_chain_t.JntToCart(joint_angles, frame_t) quat = frame_t.M.GetQuaternion() pose = PoseStamped() pose.header.frame_id = 'BASE' pose.header.stamp = rospy.Time.now() pose.pose.position.x = frame_t.p[0] pose.pose.position.y = frame_t.p[1] pose.pose.position.z = frame_t.p[2]+baseHeight pose.pose.orientation.x = quat[0] pose.pose.orientation.y = quat[1] pose.pose.orientation.z = quat[2] pose.pose.orientation.w = quat[3] pubP.publish(pose) marker = Marker() marker.header.frame_id = 'BASE' marker.header.stamp = rospy.Time.now() marker.type = marker.CUBE marker.action = marker.ADD marker.pose.orientation.w = 1 marker.scale.x = 0.2 marker.scale.y = 0.2 marker.scale.z = 0.2 marker.pose.position.x = frame_t.p[0] marker.pose.position.y = frame_t.p[1] marker.pose.position.z = frame_t.p[2]+baseHeight marker.pose.orientation.x = quat[0] marker.pose.orientation.y = quat[1] marker.pose.orientation.z = quat[2] marker.pose.orientation.w = quat[3] marker.color.a = 0.5 marker.color.r = 0.5 marker.color.g = 0.4 marker.color.b = 1 pubM.publish(marker)
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, urdf, start, end): r = URDF.from_xml_string(hacky_urdf_parser_fix(urdf)) tree = kdl_tree_from_urdf_model(r) self.chain = tree.getChain(start, end) self.fksolver = PyKDL.ChainFkSolverPos_recursive(self.chain)
def forward_kinematics(data): chain = PyKDL.Chain() joint_movement = [PyKDL.Joint.RotZ, PyKDL.Joint.RotZ,PyKDL.Joint.TransZ] n_joints = len(params.keys()) for i in range(n_joints): _, d, alpha, th = params['i'+str(i+1)] try: a, _, _, _ = params['i'+str(i+2)] except: a, _ = 0, 0 alpha, a, d, th = float(alpha), float(a), float(d), float(th) frame = PyKDL.Frame() joint = PyKDL.Joint(joint_movement[i]) frame = frame.DH(a, alpha, d, th) segment = PyKDL.Segment(joint, frame) chain.addSegment(segment) joints = PyKDL.JntArray(n_joints) for i in range(n_joints): min_joint, max_joint = scope["joint"+str(i+1)] if min_joint <= data.position[i] <= max_joint: if i == 2: joints[i] = -data.position[i] else: joints[i] = data.position[i] else: rospy.logwarn("Wrong joint value") return fk=PyKDL.ChainFkSolverPos_recursive(chain) finalFrame=PyKDL.Frame() fk.JntToCart(joints,finalFrame) quaterions = finalFrame.M.GetQuaternion() pose = PoseStamped() pose.header.frame_id = 'base_link' pose.header.stamp = rospy.Time.now() pose.pose.position.x = finalFrame.p[0] pose.pose.position.y = finalFrame.p[1] pose.pose.position.z = finalFrame.p[2] pose.pose.orientation.x = quaterions[3] pose.pose.orientation.y = quaterions[2] pose.pose.orientation.z = quaterions[1] pose.pose.orientation.w = quaterions[0] pub.publish(pose) marker = Marker() marker.header.frame_id = 'base_link' marker.type = marker.CUBE marker.action = marker.ADD marker.pose.orientation.w = 1 time = rospy.Duration() marker.lifetime = time marker.scale.x = 0.09 marker.scale.y = 0.09 marker.scale.z = 0.09 marker.pose.position.x = finalFrame.p[0]; marker.pose.position.y = finalFrame.p[1]; marker.pose.position.z = finalFrame.p[2]; marker.pose.orientation.x = quaterions[3]; marker.pose.orientation.y = quaterions[2]; marker.pose.orientation.z = quaterions[1]; marker.pose.orientation.w = quaterions[0]; marker.color.a = 0.7 marker.color.r = 0.2 marker.color.g = 0.8 marker.color.b = 0.6 marker_pub.publish(marker)
joint1_data_dmp = train_set_dmp[:, 1] joint2_data_dmp = train_set_dmp[:, 2] joint3_data_dmp = train_set_dmp[:, 3] joint4_data_dmp = train_set_dmp[:, 4] joint5_data_dmp = train_set_dmp[:, 5] joint6_data_dmp = train_set_dmp[:, 6] end_frame = PyKDL.Frame() end_frame_dmp = PyKDL.Frame() baxter = URDF.from_parameter_server(key='robot_description') base_link = baxter.get_root() limb = "right" tip_link = limb + '_gripper' kdl_tree = kdl_tree_from_urdf_model(baxter) arm_chain = kdl_tree.getChain(base_link, tip_link) fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(arm_chain) Jnt_list = PyKDL.JntArray(7) Jnt_list_dmp = PyKDL.JntArray(7) pose = [] pose_dmp = [] for i in range(train_len): Jnt_list[0] = joint0_data[i] Jnt_list[1] = joint1_data[i] Jnt_list[2] = joint2_data[i] Jnt_list[3] = joint3_data[i] Jnt_list[4] = joint4_data[i] Jnt_list[5] = joint5_data[i] Jnt_list[6] = joint6_data[i] fk_p_kdl.JntToCart(Jnt_list, end_frame) pos = end_frame.p rot = PyKDL.Rotation(end_frame.M)
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')
if kdl_model[0]: kdl_model = kdl_model[1] else: raise ValueError("Error during the parsing") ##################### # Chain, FK, and IK # ##################### #define the kinematic chain chain_RHand = kdl_model.getChain('DWYTorso', 'RForearm') #define the Forward Kinematic solver FK_RHand = kdl.ChainFkSolverPos_recursive(chain_RHand) #define the Inverse Kinematic solver IK_RHand = kdl.ChainIkSolverPos_LMA(chain_RHand) #IKV_RHand = kdl.ChainIkSolverVel_pinv(chain_RHand) #IK_RHand = kdl.ChainIkSolverPos_NR(chain_RHand, FK_RHand, IKV_RHand) ############## # Gazebo/ROS # ############## #Define arm Rarm_joints = ['RShSag', 'RShLat', 'RShYaw', 'RElbj', 'RForearmPlate'] Enu_Rarm = {i: s for i,s in enumerate(Rarm_joints)}
def __init__(self, T=20, weight=[1.0, 1.0], verbose=True): #initialize model self.chain = PyKDL.Chain() self.chain.addSegment( PyKDL.Segment( "Base", PyKDL.Joint(PyKDL.Joint.None), PyKDL.Frame(PyKDL.Vector(0.0, 0.0, 0.042)), PyKDL.RigidBodyInertia( 0.08659, PyKDL.Vector(0.00025, 0.0, -0.02420), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0)))) self.chain.addSegment( PyKDL.Segment( "Joint1", PyKDL.Joint(PyKDL.Joint.RotZ), PyKDL.Frame(PyKDL.Vector(0.0, -0.019, 0.028)), PyKDL.RigidBodyInertia( 0.00795, PyKDL.Vector(0.0, 0.019, -0.02025), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0)))) self.chain.addSegment( PyKDL.Segment( "Joint2", PyKDL.Joint(PyKDL.Joint.RotY), PyKDL.Frame(PyKDL.Vector(0.0, 0.019, 0.0405)), PyKDL.RigidBodyInertia( 0.09312, PyKDL.Vector(0.00000, -0.00057, -0.02731), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0)))) self.chain.addSegment( PyKDL.Segment( "Joint3", PyKDL.Joint(PyKDL.Joint.RotZ), PyKDL.Frame(PyKDL.Vector(0.024, -0.019, 0.064)), PyKDL.RigidBodyInertia( 0.19398, PyKDL.Vector(-0.02376, 0.01864, -0.02267), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0)))) self.chain.addSegment( PyKDL.Segment( "Joint4", PyKDL.Joint("minus_RotY", PyKDL.Vector(0, 0, 0), PyKDL.Vector(0, -1, 0), PyKDL.Joint.RotAxis), PyKDL.Frame(PyKDL.Vector(0.064, 0.019, 0.024)), PyKDL.RigidBodyInertia( 0.09824, PyKDL.Vector(-0.02099, 0.0, -0.01213), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0)))) self.chain.addSegment( PyKDL.Segment( "Joint5", PyKDL.Joint(PyKDL.Joint.RotX), PyKDL.Frame(PyKDL.Vector(0.0405, -0.019, 0.0)), PyKDL.RigidBodyInertia( 0.09312, PyKDL.Vector(-0.01319, 0.01843, 0.0), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0)))) self.chain.addSegment( PyKDL.Segment( "Joint6", PyKDL.Joint("minus_RotY", PyKDL.Vector(0, 0, 0), PyKDL.Vector(0, -1, 0), PyKDL.Joint.RotAxis), PyKDL.Frame(PyKDL.Vector(0.064, 0.019, 0.0)), PyKDL.RigidBodyInertia( 0.09824, PyKDL.Vector(-0.02099, 0.0, 0.01142), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0)))) self.chain.addSegment( PyKDL.Segment( "Joint7", PyKDL.Joint(PyKDL.Joint.RotX), PyKDL.Frame(PyKDL.Vector(0.14103, 0.0, 0.0)), PyKDL.RigidBodyInertia( 0.26121, PyKDL.Vector(-0.09906, 0.00146, -0.00021), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0)))) self.min_position_limit = [ -160.0, -90.0, -160.0, -90.0, -160.0, -90.0, -160.0 ] self.max_position_limit = [ 160.0, 90.0, 160.0, 90.0, 160.0, 90.0, 160.0 ] self.min_joint_position_limit = PyKDL.JntArray(7) self.max_joint_position_limit = PyKDL.JntArray(7) for i in range(0, 7): self.min_joint_position_limit[ i] = self.min_position_limit[i] * pi / 180 self.max_joint_position_limit[ i] = self.max_position_limit[i] * pi / 180 self.fksolver = PyKDL.ChainFkSolverPos_recursive(self.chain) self.iksolver = PyKDL.ChainIkSolverVel_pinv(self.chain) self.iksolverpos = PyKDL.ChainIkSolverPos_NR_JL( self.chain, self.min_joint_position_limit, self.max_joint_position_limit, self.fksolver, self.iksolver, 100, 1e-6) #parameter self.T = T self.weight_u = weight[0] self.weight_x = weight[1] self.verbose = verbose self.nj = self.chain.getNrOfJoints() self.q_init = np.zeros(self.nj) self.q_out = np.zeros(self.nj) ret, self.dest, self.q_out = self.generate_dest() self.fin_position = self.dest.p return
def limbPose(kdl_tree, base_link, limb_interface, limb='right'): tip_link = limb + '_gripper' tip_frame = PyKDL.Frame() arm_chain = kdl_tree.getChain(base_link, tip_link) # Baxter Interface Limb Instances #limb_interface = baxter_interface.Limb(limb) joint_names = limb_interface.joint_names() num_jnts = len(joint_names) if limb == 'right': limb_link = [ 'base', 'torso', 'right_arm_mount', 'right_upper_shoulder', 'right_lower_shoulder', 'right_upper_elbow', 'right_lower_elbow', 'right_upper_forearm', 'right_lower_forearm', 'right_wrist', 'right_hand', 'right_gripper_base', 'right_gripper' ] else: limb_link = [ 'base', 'torso', 'left_arm_mount', 'left_upper_shoulder', 'left_lower_shoulder', 'left_upper_elbow', 'left_lower_elbow', 'left_upper_forearm', 'left_lower_forearm', 'left_wrist', 'left_hand', 'left_gripper_base', 'left_gripper' ] limb_frame = [] limb_chain = [] limb_pose = [] limb_fk = [] for idx in xrange(arm_chain.getNrOfSegments()): linkname = limb_link[idx] limb_frame.append(PyKDL.Frame()) limb_chain.append(kdl_tree.getChain(base_link, linkname)) limb_fk.append( PyKDL.ChainFkSolverPos_recursive( kdl_tree.getChain(base_link, linkname))) # get the joint positions cur_type_values = limb_interface.joint_angles() while len(cur_type_values) != 7: print "Joint angles error!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" cur_type_values = limb_interface.joint_angles() kdl_array = PyKDL.JntArray(num_jnts) for idx, name in enumerate(joint_names): kdl_array[idx] = cur_type_values[name] limb_joint = [ PyKDL.JntArray(1), PyKDL.JntArray(2), PyKDL.JntArray(3), PyKDL.JntArray(4), PyKDL.JntArray(5), PyKDL.JntArray(6), PyKDL.JntArray(7) ] for i in range(7): for j in range(i + 1): limb_joint[i][j] = kdl_array[j] for i in range(arm_chain.getNrOfSegments()): joint_array = limb_joint[limb_chain[i].getNrOfJoints() - 1] limb_fk[i].JntToCart(joint_array, limb_frame[i]) pos = limb_frame[i].p rot = PyKDL.Rotation(limb_frame[i].M) rot = rot.GetQuaternion() limb_pose.append([pos[0], pos[1], pos[2]]) return np.asarray(limb_pose), kdl_array
chain.addSegment(kdl.Segment(kdlJoint, frame)) print("Joints:", chain.getNrOfJoints()) output_frame = kdl.Frame() jointParameters = kdl.JntArray(chain.getNrOfJoints()) jointParameters[0] = -0.57 jointParameters[1] = 0.12 jointParameters[2] = 0.75 jointParameters[3] = 0.71 # jointParameters[4] = 0 # jointParameters[5] = 0.785 # jointParameters[6] = -0.06 FKSolver = kdl.ChainFkSolverPos_recursive(chain) FKSolver.JntToCart(jointParameters, output_frame) print("Final frame :---") print(output_frame) IKPosSolver = kdl.ChainIkSolverPos_LMA(chain) desired_q = kdl.JntArray(chain.getNrOfJoints()) zero_q = kdl.JntArray(chain.getNrOfJoints()) tip_frame = output_frame #kdl.Frame() IKPosSolver.CartToJnt(zero_q, tip_frame, desired_q) print("desired_q:--") print(desired_q)
import numpy as np import math 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):
def callback(data): # <PyKDL.Chain object at 0x7f29da463640> kdlChain = kdl.Chain() # frame #[[ 1, 0, 0; # 0, 1, 0; # 0, 0, 1] #[ 0, 0, 0]] frame = kdl.Frame() poseR = PoseStamped() with open('in.json', 'r') as file: # we open our file and read parameters params = json.loads(file.read()) fRow = params["firstRow"] sRow = params["secondRow"] tRow = params["thirdRow"] # in .json file: a, d, alpha, theta # in frame.DH function: a, alpha, d, theta kdlChain.addSegment( kdl.Segment(kdl.Joint(kdl.Joint.TransZ), frame.DH(sRow[0], 0, fRow[1], fRow[3]))) kdlChain.addSegment( kdl.Segment(kdl.Joint(kdl.Joint.RotZ), frame.DH(tRow[0], 0, 0, sRow[3]))) # 0.5 because the length of the end elements kdlChain.addSegment( kdl.Segment(kdl.Joint(kdl.Joint.RotZ), frame.DH(0.5, 0, 0, 0))) jointPos = kdl.JntArray(kdlChain.getNrOfJoints()) jointPos[0] = data.position[0] jointPos[1] = data.position[1] jointPos[2] = data.position[2] forvKin = kdl.ChainFkSolverPos_recursive(kdlChain) eeFrame = kdl.Frame() forvKin.JntToCart(jointPos, eeFrame) quaternion = eeFrame.M.GetQuaternion() poseR.header.frame_id = 'base_link' poseR.header.stamp = rospy.Time.now() poseR.pose.position.x = eeFrame.p[0] poseR.pose.position.y = eeFrame.p[1] poseR.pose.position.z = eeFrame.p[2] poseR.pose.orientation.x = quaternion[0] poseR.pose.orientation.y = quaternion[1] poseR.pose.orientation.z = quaternion[2] poseR.pose.orientation.w = quaternion[3] publisher.publish(poseR)
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 self._kdl_tree = kdl_tree_from_urdf_model(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) # 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) # 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)
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
def __init__(self, chain): self.chain = chain self.fksolver = PyKDL.ChainFkSolverPos_recursive(self.chain) self.jac_solver = PyKDL.ChainJntToJacSolver(self.chain) self.jacobian = PyKDL.Jacobian(self.chain.getNrOfJoints()) self.joints = self.get_joints()
def forward_kinematics(data): if not correct(data): rospy.logerr('Incorrect position! ' + str(data)) return kdlChain = kdl.Chain() frameFactory = kdl.Frame(); frame0 = frameFactory.DH(0, 0, 0, 0); joint0 = kdl.Joint(kdl.Joint.None) kdlChain.addSegment(kdl.Segment(joint0, frame0)) a, d, al, th = params['i2'] al, a, d, th = float(al), float(a), float(d), float(th) frame1 = frameFactory.DH(a, al, d, th) joint1 = kdl.Joint(kdl.Joint.RotZ) kdlChain.addSegment(kdl.Segment(joint1, frame1)) a, d, al, th = params['i1'] al, a, d, th = float(al), float(a), float(d), float(th) frame2 = frameFactory.DH(a, al, d, th) joint2 = kdl.Joint(kdl.Joint.RotZ) kdlChain.addSegment(kdl.Segment(joint2, frame2)) a, d, al, th = params['i3'] al, a, d, th = float(al), float(a), float(d), float(th) frame3 = frameFactory.DH(a, al, d, th) joint3 = kdl.Joint(kdl.Joint.TransZ) kdlChain.addSegment(kdl.Segment(joint3, frame3)) jntAngles = kdl.JntArray(kdlChain.getNrOfJoints()) jntAngles[0] = data.position[0] jntAngles[1] = data.position[1] jntAngles[2] = -data.position[2] - d fksolver = kdl.ChainFkSolverPos_recursive(kdlChain) eeFrame = kdl.Frame() fksolver.JntToCart(jntAngles, eeFrame) quaternion = eeFrame.M.GetQuaternion() pose = PoseStamped() pose.header.frame_id = 'base_link' pose.header.stamp = rospy.Time.now() pose.pose.position.x = eeFrame.p[0] pose.pose.position.y = eeFrame.p[1] pose.pose.position.z = eeFrame.p[2] pose.pose.orientation.x = quaternion[0] pose.pose.orientation.y = quaternion[1] pose.pose.orientation.z = quaternion[2] pose.pose.orientation.w = quaternion[3] pub.publish(pose) marker = Marker() marker.header.frame_id = 'base_link' marker.type = marker.SPHERE marker.action = marker.ADD marker.pose.orientation.w = 1 time = rospy.Duration() marker.lifetime = time marker.scale.x = 0.07 marker.scale.y = 0.07 marker.scale.z = 0.07 marker.pose.position.x = eeFrame.p[0] marker.pose.position.y = eeFrame.p[1] marker.pose.position.z = eeFrame.p[2] marker.pose.orientation.x = quaternion[0] marker.pose.orientation.y = quaternion[1] marker.pose.orientation.z = quaternion[2] marker.pose.orientation.w = quaternion[3] marker.color.a = 0.7 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 marker_pub.publish(marker)