def __init__(self):
dir = os.path.dirname(__file__)
urdfFile = os.path.join(dir, "./withRootTongsAssembly.URDF")
(ok, tree) = urdf.treeFromFile(urdfFile)
self.tree = tree
self.chain1 = tree.getChain('base_link', 'upperForceSensor')
self.fksolver1 = kdl.ChainFkSolverPos_recursive(self.chain1)
self.chain2 = tree.getChain('base_link', 'lowerForceSensor')
self.fksolver2 = kdl.ChainFkSolverPos_recursive(self.chain2)
self.getTransforms(0)
def urdf_load(urdfString,
startJoint,
endJoint,
full_joint_list,
fixed_ee_joint=None,
Debug=False):
'''
Takes in a urdf file and parses that into different object representations of the robot arm
:param urdfString: (string) file path to the urdf file. example: < 'mico.urdf' >
:param startJoint: (string) name of the starting joint in the chain
:param endJoint: (string) name of the final joint in the chain
NOTE: this is the final ROTATING joint in the chain, for a fixed joint on the end effector, add this as the fixed joint!
:param fixed_ee_joint (string) name of the fixed joint after end joint in the chain. This is read in just to get a final
displacement on the chain, i.e. usually just to add in an extra displacement offset for the end effector
:return: returns the robot parsed object from urdf_parser, Mike's "arm" version of the robot arm, as well as the kdl tree
'''
if urdfString == '':
urdf_robot = URDF.from_parameter_server()
(ok, kdl_tree) = pyurdf.treeFromParam('/robot_description')
else:
urdf_robot = URDF.from_xml_file(urdfString)
(ok, kdl_tree) = pyurdf.treeFromFile(urdfString)
if not (startJoint == '' or endJoint == ''):
chain = kdl_tree.getChain(startJoint, endJoint)
if full_joint_list == ():
arm, arm_c = convertToArm(urdf_robot,
startJoint,
endJoint,
fixed_ee_joint,
Debug=Debug)
else:
arm, arm_c = convertToArmJointList(urdf_robot,
full_joint_list,
fixed_ee_joint,
Debug=Debug)
if Debug:
o = open('out', 'w')
o.write(str(urdf_robot))
return urdf_robot, arm, arm_c, kdl_tree
def prepare_kin_dyn(self):
flag, self.tree = kdl_parser.treeFromFile("data/jelly/jelly.urdf")
chain = self.tree.getChain(self.base_link, self.bl_link)
self.fk_bl = kdl.ChainFkSolverPos_recursive(chain)
self.jac_bl = kdl.ChainJntToJacSolver(chain)
chain = self.tree.getChain(self.base_link, self.br_link)
self.fk_br = kdl.ChainFkSolverPos_recursive(chain)
self.jac_br = kdl.ChainJntToJacSolver(chain)
chain = self.tree.getChain(self.base_link, self.fl_link)
self.fk_fl = kdl.ChainFkSolverPos_recursive(chain)
self.jac_fl = kdl.ChainJntToJacSolver(chain)
chain = self.tree.getChain(self.base_link, self.fr_link)
self.fk_fr = kdl.ChainFkSolverPos_recursive(chain)
self.jac_fr = kdl.ChainJntToJacSolver(chain)
# convention front_right, front_left, back_right, back_left
self.jac_list = [self.jac_fl, self.jac_fr, self.jac_bl, self.jac_br]
self.fk_list = [self.fk_fl, self.fk_fr, self.fk_bl, self.fk_br]
joints = kdl.JntArray(3)
joints[0] = 0
joints[1] = 0
joints[2] = 0
frame = kdl.Frame()
jk_list = self.fk_list
print(jk_list[0].JntToCart(joints, frame))
print(frame)
print(jk_list[1].JntToCart(joints, frame))
print(frame)
print(jk_list[2].JntToCart(joints, frame))
print(frame)
print(jk_list[3].JntToCart(joints, frame))
print(frame)
import urdf2casadi.urdfparser as u2c from urdf2casadi.geometry import plucker import numpy as np import PyKDL as kdl import kdl_parser_py.urdf as kdlurdf import pybullet as pb path_to_urdf = "../../urdf/gantry.urdf" root = "gantry_link_base" tip = "gantry_tool0" #get robot models #kdl ok, ur_tree = kdlurdf.treeFromFile(path_to_urdf) gantry_kdl = ur_tree.getChain(root,tip) #rbdl gantry_rbdl = rbdl.loadModel(path_to_urdf) #u2c gantry = u2c.URDFparser() gantry.from_file(path_to_urdf) #pybullet sim = pb.connect(pb.DIRECT) gantry_pb = pb.loadURDF(path_to_urdf, useFixedBase=True, flags = pb.URDF_USE_INERTIA_FROM_FILE) pb.setGravity(0, 0, -9.81) #joint info
import os # For current directory
import urdf2casadi.urdfparser as u2c
from urdf2casadi.geometry import plucker
import numpy as np
import PyKDL as kdl
import kdl_parser_py.urdf as kdlurdf
import pybullet as pb
root = 'base_link'
tip = 'tilt_link'
path_to_urdf = '../../urdf/pantilt.urdf'
#get robot models
#kdl
ok, snake_tree = kdlurdf.treeFromFile(path_to_urdf)
snake_chain = snake_tree.getChain(root, tip)
#rbdl
snake_rbdl = rbdl.loadModel(path_to_urdf)
#u2c
snake = u2c.URDFparser()
robot_desc = 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)
#from urdf_parser_py.urdf import URDF
import kdl_parser_py.urdf as KDLParserURDF
import PyKDL as kdl
###################
# Load URDF Model #
###################
#Path to the urdf file
coman_urdf = '/data/emily/robot_catkin_ws/src/IIT_ROBOT/coman/coman_urdf/urdf/coman.urdf'
#ROS rate
ros_rate = 100
#load urdf model
kdl_model = KDLParserURDF.treeFromFile(coman_urdf)
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)
import casadi as cs
from urdf_parser_py.urdf import URDF, Pose
import os # For current directory
import urdf2casadi.urdfparser as u2c
import numpy as np
import PyKDL as kdl
import kdl_parser_py.urdf as kdlurdf
root = 'base_link'
tip = 'link60'
#root = "calib_kuka_arm_base_link"
#tip = "kuka_arm_7_link"
ok, ur_tree = kdlurdf.treeFromFile(
'/home/lmjohann/urdf2casadi/examples/timing/urdf4timing/kuka.urdf')
#ok, ur_tree = kdlurdf.treeFromFile('/home/lmjohann/urdf2casadi/examples/urdf/kuka.urdf')
kuka_chain = ur_tree.getChain(root, tip)
kuka = u2c.URDFparser()
#kuka.from_file("/home/lmjohann/urdf2casadi/examples/urdf/kuka.urdf")
kuka.from_file(
"/home/lmjohann/urdf2casadi/examples/timing/urdf4timing/60dof.urdf")
jointlist, names, q_max, q_min = kuka.get_joint_info(root, tip)
n_joints = kuka.get_n_joints(root, tip)
q_kdl = kdl.JntArray(n_joints)
q = [None] * n_joints
gravity_kdl = kdl.Vector()
gravity_kdl[2] = -9.81
import casadi as cs
from urdf_parser_py.urdf import URDF, Pose
import os # For current directory
import urdf2casadi.urdfparser as u2c
import numpy as np
import PyKDL as kdl
import kdl_parser_py.urdf as kdlurdf
root = "calib_kuka_arm_base_link"
tip = "kuka_arm_7_link"
ok, ur_tree = kdlurdf.treeFromFile('../../urdf/kuka.urdf')
kuka_chain = ur_tree.getChain(root, tip)
kuka = u2c.URDFparser()
kuka.from_file("../../urdf/kuka.urdf")
jointlist, names, q_max, q_min = kuka.get_joint_info(root, tip)
n_joints = kuka.get_n_joints(root, tip)
q_kdl = kdl.JntArray(n_joints)
q = [None] * n_joints
gravity_kdl = kdl.Vector()
gravity_kdl[2] = -9.81
gravity_u2c = [0., 0., -9.81]
G_kdl = kdl.JntArray(n_joints)
G_sym = kuka.get_gravity_rnea(root, tip, gravity_u2c)
error = np.zeros(n_joints)
import kdl_parser_py.urdf as KDLParserURDF
import PyKDL as kdl
###################
# Load URDF Model #
###################
#Path to the urdf file
centauro_urdf = '/data/emily/robot_catkin_ws/src/IIT_ROBOT/centauro-simulator/centauro/centauro_urdf/urdf/centauro.urdf.xacro'
#ROS rate
ros_rate = 100
#load urdf model
kdl_model = KDLParserURDF.treeFromFile(centauro_urdf)
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
import casadi as cs
import rbdl
from urdf_parser_py.urdf import URDF, Pose
import os # For current directory
import urdf2casadi.urdfparser as u2c
from urdf2casadi.geometry import plucker
import numpy as np
import PyKDL as kdl
import kdl_parser_py.urdf as kdlurdf
import pybullet as pb
path_to_urdf = "/home/lmjohann/urdf2casadi/examples/urdf/gantry.urdf"
root = "gantry_link_base"
tip = "gantry_tool0"
#kdl
ok, gantry_tree = kdlurdf.treeFromFile(path_to_urdf)
gantry_chain = gantry_tree.getChain(root, tip)
#rbdl
gantry_rbdl = rbdl.loadModel(path_to_urdf)
#u2c
gantry = u2c.URDFparser()
gantry.from_file(path_to_urdf)
#pybullet
sim = pb.connect(pb.DIRECT)
gantry_pb = pb.loadURDF(path_to_urdf,
useFixedBase=True,
flags=pb.URDF_USE_INERTIA_FROM_FILE)
pb.setGravity(0, 0, -9.81)
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
# set joint velocities
# robot.set_joint_velocities(dq)
# set joint positions
q = q[qIdx] + dq * dt
robot.set_joint_positions(q, joint_ids=joint_ids)
# step in simulation
world.step(sleep_dt=dt)
##################
# IK using PyKDL #
##################
elif solver_flag == 2:
print("Using PyKDL:")
model = kdl_parser.treeFromFile(urdf)
if model[0]:
model = model[1]
else:
raise ValueError("Error during the parsing")
# define the kinematic chain
chain = model.getChain(base_name, end_effector_name)
print("Number of joints in the chain: {}".format(chain.getNrOfJoints()))
# define the FK solver
FK = kdl.ChainFkSolverPos_recursive(chain)
# define the IK Solver
# IKV = kdl.ChainIkSolverVel_pinv(chain)
# IK = kdl.ChainIkSolverPos_NR(chain, FK, IKV)
