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)
