def coriolis(self, q, dq):
c_kdl = kdl.JntArray(self.num_joints)
self._dyn_kdl.JntToCoriolis(joint_list_to_kdl(q),
joint_list_to_kdl(dq), c_kdl)
return joint_kdl_to_list(c_kdl)
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 __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 compute_ff(self):
stance = self.get_stance_legs()
joint_pos = np.array(self.joint_positions).copy()
p_ = []
for i in range(4):
frame = kdl.Frame()
joints = kdl.JntArray(3)
joints[0] = joint_pos[i + 0]
joints[1] = joint_pos[i + 1]
joints[2] = joint_pos[i + 2]
self.fk_list[i].JntToCart(joints, frame)
frame_p = np.array([frame.p[0], frame.p[1], frame.p[2]])
foot_pos = frame_p
p_.append(foot_pos)
acc = np.zeros(3)
constraints = []
constraints_b = []
equality_const = []
equality_const_b = []
theta = np.pi / 4
F_normal = []
num_stance = 0
for i, s in enumerate(stance):
if s:
const = np.zeros(12)
const[i * 3 + 0] = 0
const[i * 3 + 1] = 0
const[i * 3 + 2] = -1
constraints.append(const)
constraints_b.append(0)
F_normal.append([0, 0, self.mass * 9.81])
num_stance = num_stance + 1
else:
const = np.zeros(12)
const[i * 3 + 0] = 1
const[i * 3 + 1] = 0
const[i * 3 + 2] = 0
equality_const.append(const)
equality_const_b.append(0)
const = np.zeros(12)
const[i * 3 + 0] = 0
const[i * 3 + 1] = 1
const[i * 3 + 2] = 0
equality_const.append(const)
equality_const_b.append(0)
const = np.zeros(12)
const[i * 3 + 0] = 0
const[i * 3 + 1] = 0
const[i * 3 + 2] = 1
equality_const.append(const)
equality_const_b.append(0)
F_normal.append([0, 0, 0])
F_normal = np.array(F_normal) / num_stance
eye3 = np.eye(3)
Atop = np.hstack((eye3, eye3, eye3, eye3))
Abot = np.hstack((rm.skew_3d(p_[0]), rm.skew_3d(p_[1]),
rm.skew_3d(p_[2]), rm.skew_3d(p_[3])))
A = np.vstack([Atop, Abot])
g = np.array([0, 0, 9.8])
bd_top = self.mass * g
bd_top = np.reshape(bd_top, (3, 1))
bd_bot = np.zeros(3)
bd_bot = np.reshape(bd_bot, (3, 1))
bd = np.vstack((bd_top, bd_bot))
P = np.eye(12)
q = np.zeros(12)
G = np.array(constraints)
h = np.array(constraints_b)
if not equality_const == []:
A = np.vstack((A, np.array(equality_const)))
b = np.vstack((bd, np.array(equality_const_b)))[:, 0]
else:
A = A
b = bd[:, 0]
# print("qwer")
# print(P)
# print(q)
# print(G)
# print(h)
# print(A)
# print(b)
F_opt = rm.quadprog_solve_qp(P, q, G=G, h=h, A=A, b=b)
# print("f_opt")
# print(F_opt)
if np.abs(np.sum(F_opt)) < 1e-5:
foot_forces = np.array(F_normal)
# foot_forces = F_opt.reshape(4, 3)
# print(foot_forces.shape)
else:
foot_forces = F_opt
# print(foot_forces.shape)
# print("ihi")
# print(foot_forces)
torques = self.force_to_torques(foot_forces, joint_pos)
# print(torques)
return torques
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
def convert_joint_angles_array_to_kdl_array(self, joint_angles_array):
num_joints = len(joint_angles_array)
kdl_array = PyKDL.JntArray(num_joints)
for i in range(num_joints):
kdl_array[i] = joint_angles_array[i]
return kdl_array
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)
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)
print("Get urdf model from parameter server")
model = URDF.from_parameter_server()
#print model
print("Loading kdl tree using urdf from parameter server")
ret, kdltree = urdf.treeFromUrdfModel(model, quiet=False)
# Get the whole kinematic chain from kdl tree
chain = kdltree.getChain(base_link, end_link)
# Initialize kdl solver for jacobian calculation
solver = kdl.ChainJntToJacSolver(chain)
jnt_array = kdl.JntArray(chain.getNrOfSegments())
# Get joints from the chain
#for i in range(chain.getNrOfSegments()):
# print chain.getSegment(i).getJoint()
jac = kdl.Jacobian(chain.getNrOfSegments())
# print(dir(kdl))
my_arr = np.zeros((6, 7))
if (solver.JntToJac(jnt_array, jac) == 0):
rospy.loginfo("Sucessfully calculated jacobian")
print jac[0, 3]
for i in range(0, 6):
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 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)
def compute_balance(self, stance):
p_c, curr_ori, joint_pos, joint_vel = self.GetState()
curr_ori = rm.quat_to_rot(curr_ori)
p_ = []
for i in range(4):
frame = kdl.Frame()
joints = kdl.JntArray(3)
joints[0] = joint_pos[i + 0]
joints[1] = joint_pos[i + 1]
joints[2] = joint_pos[i + 2]
self.fk_list[i].JntToCart(joints, frame)
frame_p = np.array([frame.p[0], frame.p[1], frame.p[2]])
foot_pos = p_c + frame_p
p_.append(foot_pos)
acc = np.zeros(3)
# orientation_error = np.log(ori_des.dot(curr_ori))
# print("angular accel: {}".format(angular_acc))
constraints = []
theta = np.pi / 4
for s in stance:
if s:
cons = np.array([[1, 0,
-np.tan(theta)], [-1, 0, -np.tan(theta)],
[0, 1, -np.tan(theta)],
[0, -1, -np.tan(theta)], [0, 0, -1]])
else:
print("error")
cons = np.array([[1, 0, 0], [-1, 0, 0], [0, 1, 0], [0, -1, 0],
[0, 0, 1], [0, 0, -1]])
constraints.append(cons)
eye3 = np.eye(3)
Atop = np.hstack((eye3, eye3, eye3, eye3))
Abot = np.hstack((rm.skew_3d(p_[0] - p_c), rm.skew_3d(p_[1] - p_c),
rm.skew_3d(p_[2] - p_c), rm.skew_3d(p_[3] - p_c)))
A = np.vstack([Atop, Abot])
g = np.array([0, 0, -9.8])
m = 13.61
bd_top = m * g
bd_top = np.reshape(bd_top)
bd_bot = np.zeros(3)
bd_bot = np.reshape(bd_bot)
bd = np.vstack((bd_top, bd_bot))
P = np.eye(12)
G = block_diag(*constraints)
num_h = G.shape[0]
h = np.zeros(num_h)
F_opt = rm.quadprog_solve_qp(P, q, G=G, h=h)
print(F_opt)
# F_opt = np.linalg.solve(np.dot(A.T, A) + 0.1*np.eye(12),np.dot(A.T, bd))
# print(F_opt)
return F_opt
def ik(self, eef_pose):
q_init = self.joint_state.q
q_sol = kdl.JntArray(self.num_joints)
self.pos_ik_solver.CartToJnt(q_init, eef_pose, q_sol)
return q_sol
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 _joints_to_kdl(self):
kdl_array = PyKDL.JntArray(self._num_jnts)
current_angles = self._limb.joint_angles()
for i, name in enumerate(self._joint_names):
kdl_array[i] = current_angles[name]
return kdl_array
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
import rospy
import kdl_parser_py.urdf
import PyKDL as kdl
import math
import actionlib
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
if __name__ == "__main__":
rospy.init_node("py_kdl_ik")
robot_description = rospy.get_param("robot_description")
(ok, tree) = kdl_parser_py.urdf.treeFromString(robot_description)
joint_names = rospy.get_param("/crane_x7/arm_controller/joints")
chain = tree.getChain("base_link", "crane_x7_gripper_base_link")
solver = kdl.ChainIkSolverPos_LMA(chain)
q_init = kdl.JntArray(chain.getNrOfJoints())
p_out = kdl.Frame(kdl.Rotation.RPY(0.0, math.radians(90.0), 0.0),
kdl.Vector(0.0, 0.0, 0.32))
q_sol = kdl.JntArray(chain.getNrOfJoints())
ret = solver.CartToJnt(q_init, p_out, q_sol)
if ret != 0:
rospy.logerr("IK Failed")
rospy.signal_shutdown("IK Failed")
ac = actionlib.SimpleActionClient(
"/crane_x7/arm_controller/follow_joint_trajectory",
FollowJointTrajectoryAction)
if not ac.wait_for_server(timeout=rospy.Duration(3.0)):
rospy.logerr("Cannot found action server")
rospy.signal_shutdown("Cannot found action server")
flg.append(True)
def callback(data):
caliculate(data.data[0], data.data[1], data.data[2])
rospy.init_node('inverse_kinematics', anonymous=True)
rospy.Subscriber("chatter", Float32MultiArray, callback)
pub = rospy.Publisher('joint_states', JointState, queue_size=10)
position = [2] * 5
position_old = [2] * 5
q_zero = None
q_zero = kdl.JntArray(chain.getNrOfJoints())
q_solved = None
def caliculate(data0, data1, data2):
global q_zero
global q_solved
print q_zero
F2 = kdl.Frame(kdl.Rotation.Quaternion(0, -0.29552, 0, 0.955337),
kdl.Vector(data0, data1, data2))
global position
global position_old
# print position[0]
#print position_old[0]
for i in range(5):
if flg[i] == True:
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):
#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')
snake = u2c.URDFparser() snake.from_file(path_to_urdf) #pybullet sim = pb.connect(pb.DIRECT) snake_pb = pb.loadURDF(path_to_urdf, useFixedBase=True, flags = pb.URDF_USE_INERTIA_FROM_FILE) #joint info jointlist, names, q_max, q_min = snake.get_joint_info(root, tip) n_joints = snake.get_n_joints(root, tip) #declarations #kdl q_kdl = kdl.JntArray(n_joints) qdot_kdl = kdl.JntArray(n_joints) gravity_kdl = kdl.Vector() gravity_kdl[2] = -9.81 C_kdl = kdl.JntArray(n_joints) g_kdl = kdl.JntArray(n_joints) #u2c & pybullet q = [None]*n_joints qdot = [None]*n_joints zeros_pb = [None]*n_joints gravity_u2c = [0, 0, -9.81] g_sym = snake.get_gravity_rnea(root, tip, gravity_u2c) C_sym = snake.get_coriolis_rnea(root, tip)
from urdf_parser_py.urdf import URDF
from pykdl_utils.kdl_parser import kdl_tree_from_urdf_model
robot = URDF.from_xml_file("/home/d/catkin_ws/src/robot_description/armc_description/urdf/armc_description.urdf")
tree = kdl_tree_from_urdf_model(robot)
print tree.getNrOfSegments()
chain = tree.getChain("base_link", "sensor_link")
print chain.getNrOfSegments()
print chain.getNrOfJoints()
#正运动学
fk = PyKDL.ChainFkSolverPos_recursive(chain)
pos = PyKDL.Frame()
q = PyKDL.JntArray(7)
for i in range(7):
q[i] = 0
q[0] = 1
fk_flag = fk.JntToCart(q, pos)
print "fk_flag", fk_flag
print "pos", pos
#逆运动学
ik_v = PyKDL.ChainIkSolverVel_pinv(chain)
ik = PyKDL.ChainIkSolverPos_NR(chain, fk, ik_v, maxiter=100, eps=math.pow(10, -9))
qq = PyKDL.JntArray(7)
qq_k = PyKDL.JntArray(7)
ik.CartToJnt(qq_k, pos, qq)
print "qq:", qq
def inverse_kinematics(self,
position,
orientation=None,
seed=None,
min_joints=None,
max_joints=None,
w_x=None,
w_q=None,
maxiter=500,
eps=1.0e-6,
with_st=False):
if w_x is None and w_q is None:
ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain)
else:
ik_v_kdl = PyKDL.ChainIkSolverVel_wdls(self._arm_chain)
if w_x is not None: ik_v_kdl.setWeightTS(w_x) #TS = Task Space
if w_q is not None: ik_v_kdl.setWeightJS(w_q) #JS = Joint Space
pos = PyKDL.Vector(position[0], position[1], position[2])
if orientation is not None:
rot = PyKDL.Rotation()
rot = rot.Quaternion(orientation[0], orientation[1],
orientation[2], orientation[3])
# Populate seed with current angles if not provided
seed_array = PyKDL.JntArray(self._num_jnts)
if seed is not None:
seed_array.resize(len(seed))
for idx, jnt in enumerate(seed):
seed_array[idx] = jnt
else:
seed_array = self.joints_to_kdl('positions')
# Make IK Call
if orientation is not None:
goal_pose = PyKDL.Frame(rot, pos)
else:
goal_pose = PyKDL.Frame(pos)
result_angles = PyKDL.JntArray(self._num_jnts)
# Make IK solver with joint limits
if min_joints is None: min_joints = self.joint_limits_lower
if max_joints is None: max_joints = self.joint_limits_upper
mins_kdl = PyKDL.JntArray(len(min_joints))
for idx, jnt in enumerate(min_joints):
mins_kdl[idx] = jnt
maxs_kdl = PyKDL.JntArray(len(max_joints))
for idx, jnt in enumerate(max_joints):
maxs_kdl[idx] = jnt
ik_p_kdl = PyKDL.ChainIkSolverPos_NR_JL(self._arm_chain, mins_kdl,
maxs_kdl, self._fk_p_kdl,
ik_v_kdl, maxiter, eps)
if ik_p_kdl.CartToJnt(seed_array, goal_pose, result_angles) >= 0:
result = np.array(list(result_angles))
if with_st: return True, result
else: return result
else:
if with_st:
result = np.array(list(result_angles))
return False, result
else:
return None
def NpToJnt(self, q):
temp = PyKDL.JntArray(self.nj)
for j in range(self.nj):
temp[j] = q[j]
return temp
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')
#pybullet
sim = pb.connect(pb.DIRECT)
snake_pb = pb.loadURDF(path_to_urdf,
useFixedBase=True,
flags=pb.URDF_USE_INERTIA_FROM_FILE)
pb.setGravity(0, 0, -9.81)
#joint info
jointlist, names, q_max, q_min = snake.get_joint_info(root, tip)
n_joints = snake.get_n_joints(root, tip)
#declarations
#kdl
q_kdl = kdl.JntArray(n_joints)
gravity_kdl = kdl.Vector()
gravity_kdl[2] = -9.81
g_kdl = kdl.JntArray(n_joints)
M_kdl = kdl.JntSpaceInertiaMatrix(n_joints)
#u2c & pybullet
q = [None] * n_joints
qdot = [None] * n_joints
zeros_pb = [None] * n_joints
M_sym = snake.get_inertia_matrix_crba(root, tip)
#rbdl
q_np = np.zeros(n_joints)
M_rbdl = (n_joints, n_joints)
M_rbdl = np.zeros(M_rbdl)
def gravity(self, q):
g_kdl = kdl.JntArray(self.num_joints)
self._dyn_kdl.JntToGravity(joint_list_to_kdl(q), g_kdl)
return joint_kdl_to_list(g_kdl)
