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)