def __init__(self, direction): self.state = [0, 0, 0, 0, 0] if direction == "left": sign = -1 else: sign = 1 self.chain = Chain( name=direction + "_arm", links=[ OriginLink(), URDFLink( name="shoulder", translation_vector=[8 * sign, 0, 0], orientation=[0, 0, math.radians(30 * sign)], rotation=[0, 0, 1], ), URDFLink(name="backarm", translation_vector=[16 * sign, 0, 0], orientation=[0, 0, math.radians(90 * sign)], rotation=[0, 0, 1]), URDFLink(name="forearm", translation_vector=[16 * sign, 0, 0], orientation=[0, 0, math.radians(30 * sign)], rotation=[0, 0, 1]), URDFLink(name="hand", translation_vector=[9 * sign, 0, 0], orientation=[0, 0, 0], rotation=[0, 0, 1]), ])
def __init__(self): self.chain = Chain( name="pupper_ergo", links=[ OriginLink(), URDFLink( name="twistybase", translation_vector=[0, 0, 0.03], orientation=[0, 0, 0], rotation=[0, 0, 1], ), URDFLink( name="shoulder", translation_vector=[0, 0, 0.0285], orientation=[0, 0, 0], rotation=[1, 0, 0], ), URDFLink( name="elbow", translation_vector=[0, 0, 0.085], orientation=[0, 0, 0], rotation=[1, 0, 0], ), URDFLink( name="tip", translation_vector=[0, 0.155, 0], orientation=[1.57, 0, 0], rotation=[1, 0, 0], ), ], )
def solve_shoulder_angles(v_o, v_new, a1, a2, b1, b2): chain = Chain( name='arm', links=[ #OriginLink(), URDFLink( name="shoulder1", translation_vector=[0, 0, 0], orientation=[0, 0, 0], bounds=b1, rotation=a1, ), URDFLink( name="shoulder2", translation_vector=[0, 0, 0], orientation=[0, 0, 0], bounds=b2, rotation=a2, ), URDFLink( name="elbow", translation_vector=v_o, orientation=[0, 0, 0], rotation=[0, 0, 0], ) ]) target_frame = np.eye(4) target_frame[:3, 3] = v_new angles = chain.inverse_kinematics(target_frame)[:2] return np.rad2deg(angles)
class ArmIK: def __init__(self): self.chain = Chain( name="pupper_ergo", links=[ OriginLink(), URDFLink( name="twistybase", translation_vector=[0, 0, 0.03], orientation=[0, 0, 0], rotation=[0, 0, 1], bounds=(-90, 90), ), URDFLink( name="shoulder", translation_vector=[0, 0, 0.0285], orientation=[0, 0, 0], rotation=[1, 0, 0], bounds=(-90, 90), ), URDFLink( name="elbow", translation_vector=[0, 0, 0.085], orientation=[0, 0, 0], rotation=[1, 0, 0], bounds=(-90, 90), ), URDFLink( name="tip", translation_vector=[0, 0.155, 0], orientation=[1.57, 0, 0], rotation=[1, 0, 0], bounds=(-90, 90), ), ], ) @staticmethod def _ik2pupperarm(joints): return np.rad2deg([joints[1], -joints[2], -joints[3], 0.0]) @staticmethod def _fk2pupperarm(joints): return np.deg2rad([joints[1], -joints[2], -joints[3], 0.0, 0.0]) @staticmethod def _makeMoveCmd(degs): print(f"move([{','.join([str(x) for x in np.around(degs, 2)])}])") def pos2joints(self, target): joints = self.chain.inverse_kinematics(target) return self._ik2pupperarm(joints) def joints2pos(self, joints): joints = self._fk2pupperarm(joints) return self.chain.forward_kinematics(joints)[:3, 3]
def __init__(self): self.my_chain = Chain.from_urdf_file("./panda_with_bound.URDF") self.my_chain_ = Chain.from_urdf_file( "./panda_with_bound_nofinger_center.URDF") self.state = [0] * 10 self.motorPosList = [0] * 7 # self.target_position = [0]*3 TargetList = [[0.5, 0, 0], [0.4, 0, 0], [0.6, 0, 0], [0.4, 0.2, 0], [0.5, 0.2, 0], [0.6, 0.2, 0], [0.4, -0.2, 0], [0.5, -0.2, 0], [0.6, -0.2, 0]] self.target_position = np.array(TargetList[random.randint(0, 8)])
def __init__(self): # Second robo, with dynamixel self.armLength = 4 self.smallArmLength = 4 lim = math.pi / 4 # lim = 7*math.pi/9 self.chain = Chain( name='arm', links=[ OriginLink(), URDFLink(name="base", translation_vector=[0, 0, 0], orientation=[0, 0, 0], rotation=[0, 0, 1], bounds=(-math.pi, math.pi)), URDFLink(name="first", translation_vector=[0, 0, 0], orientation=[0, 0, 0], rotation=[0, 1, 0], bounds=(-lim, lim)), URDFLink(name="second", translation_vector=[0, 0, self.armLength], orientation=[0, 0, 0], rotation=[0, -1, 0], bounds=(-lim, lim)), URDFLink(name="third", translation_vector=[0, 0, self.armLength], orientation=[0, 0, 0], rotation=[0, 1, 0], bounds=(-lim, lim)), URDFLink(name="fourth", translation_vector=[0, 0, self.armLength], orientation=[0, 0, 0], rotation=[0, -1, 0], bounds=(-lim, lim)), URDFLink( name="tip", translation_vector=[0, 0, self.smallArmLength], orientation=[0, 0, 0], rotation=[0, 0, 0], ) ]) self.xMin = -3 * self.armLength - self.smallArmLength self.xMax = 3 * self.armLength + self.smallArmLength self.yMin = -3 * self.armLength - self.smallArmLength self.yMax = 3 * self.armLength + self.smallArmLength self.zMin = 0 self.zMax = 3 * self.armLength + self.smallArmLength self.radToDegreeFactor = 180 / math.pi self.degreeToRadFactor = math.pi / 180 self.numberOfLinks = len(self.chain.links)
class Arm: def __init__(self, direction): self.state = [0, 0, 0, 0, 0] if direction == "left": sign = -1 else: sign = 1 self.chain = Chain( name=direction + "_arm", links=[ OriginLink(), URDFLink( name="shoulder", translation_vector=[8 * sign, 0, 0], orientation=[0, 0, math.radians(30 * sign)], rotation=[0, 0, 1], ), URDFLink(name="backarm", translation_vector=[16 * sign, 0, 0], orientation=[0, 0, math.radians(90 * sign)], rotation=[0, 0, 1]), URDFLink(name="forearm", translation_vector=[16 * sign, 0, 0], orientation=[0, 0, math.radians(30 * sign)], rotation=[0, 0, 1]), URDFLink(name="hand", translation_vector=[9 * sign, 0, 0], orientation=[0, 0, 0], rotation=[0, 0, 1]), ]) def get_positions(self): rv = [] positions = self.chain.forward_kinematics(self.state, full_kinematics=True) for position in positions[1:]: xyEtc = position[:, 3] x = xyEtc[0] y = xyEtc[1] rv.append((x, y)) return rv def go_directly_to_position(self, x, y, book): rad = math.radians(book.openness / 2) if (x < book.center_x_cm): rad *= -1 joint_positions = self.chain.inverse_kinematics( target_position=[x, y, 0], target_orientation=[0, 0, 0.2]) self.state = joint_positions
class KSOLVER: def __init__(self, *args, **kwargs): self.chain = Chain( name='left_arm', links=[ OriginLink(), URDFLink( name="shoulder_left", translation_vector=[0.13182, -2.77556e-17, 0.303536], orientation=[-0.752186, -0.309384, -0.752186], rotation=[0.707107, 0, 0.707107], ), URDFLink( name="proximal_left", translation_vector=[-0.0141421, 0, 0.0424264], orientation=[-7.77156e-16, 7.77156e-16, 5.55112e-17], rotation=[-0.707107, 0, 0.707107], ), URDFLink( name="distal_left", translation_vector=[0.193394, -0.0097, 0.166524], orientation=[1.66533e-16, -7.21645e-16, 3.88578e-16], rotation=[0, -1, 0], ) ]) def solve(self, wrist_postion): """Performs inverse kinematics and returns joint angles in rads For now uses one target vector at the wrist position for inverse kinematics. For the future, should use additional target vectors and match the elbow """ # TODO: Fix coordinate system transform for extra degree of freedom rotation # Setup target vector with target position at wrist target_vector = np.array( [wrist_postion[0], wrist_postion[1], wrist_postion[2]]) target_frame = np.eye(4) target_frame[:3, 3] = target_vector joint_angles = self.chain.inverse_kinematics(target_frame)[1:3] print('Joint angles:', joint_angles) print('Computed position vector:', self.chain.forward_kinematics(self.chain.inverse_kinematics(target_frame))[:3, 3], \ 'Original position vector:', target_frame[:3, 3]) joints = self.chain.inverse_kinematics(target_frame)[1:3] return np.array([0, joints[1]])
def backhoe(): backhoe_def = BackhoeDefinition() # Actuators # Origin is at bottom link of boom piston chain = Chain(name='backhoe', links=[ OriginLink(), URDFLink( name="boom", bounds=(0, 90), translation_vector=[0, 0, 0], orientation=[0, 0, 0], rotation=[0, 1, 0], ), URDFLink( name="stick", translation_vector=[25, 0, 0], orientation=[0, 0, 0], rotation=[0, 1, 0], ), URDFLink( name="bucket", translation_vector=[22, 0, 0], orientation=[0, 0, 0], rotation=[0, 1, 0], ) ]) return chain pass
def __init__(self, active_joints=[1, 1, 1, 1], base_translation=[0, 0, 0.84], # x, y, z base_orientation=[0, 0, np.pi/2], # x, y, z tool_translation=[0, 0, 0], tool_orientation=[0, 0, 0]): links=[OriginLink(), URDFLink(name="wam/links/base", translation_vector=base_translation, # translation of frame orientation=base_orientation, # orientation of frame rotation=[0, 0, 0]), # joint axis [0, 0, 0] -> no joint URDFLink(name="wam/links/shoulder_yaw", translation_vector=[0, 0, 0.16], orientation=[0, 0, 0], rotation=[0, 0, 1]), URDFLink(name="wam/links/shoulder_pitch", translation_vector=[0, 0, 0.186], orientation=[0, 0, 0], rotation=[1, 0, 0]), URDFLink(name="wam/links/shoulder_roll", translation_vector=[0, 0, 0], orientation=[0, 0, 0], rotation=[0, 0, 1]), URDFLink(name="wam/links/upper_arm", translation_vector=[0, -0.045, 0.550], orientation=[0, 0, 0], rotation=[1, 0, 0]), URDFLink(name="wam/links/tool_base_wo_plate", translation_vector=[0, 0.045, 0.350], orientation=[0, 0, 0], rotation=[0, 0, 0]), URDFLink(name="wam/links/tool_base_w_plate", translation_vector=[0, 0, 0.008], orientation=[0, 0, 0], rotation=[0, 0, 0]), URDFLink(name="wam/links/tool", translation_vector=tool_translation, orientation=tool_orientation, rotation=[0, 0, 0]) ] self.all_joints = [False, False, True, True, True, True, False, False, False] self.active_joints = list(map(lambda x:x==1, active_joints)) self.active_links = [False, False, *self.active_joints, False, False, False] Chain.__init__(self, name='wam4', active_links_mask=self.active_links, links=links)
def __init__(self): self.urdf = Chain.from_urdf_file("URDF/dofbot.urdf") self.dofbot = Arm_Lib.Arm_Device() self.target = [] self.joints = [] self.is_closed = False self.gripper = OPEN_GRIPPER self.standby_pose = [90,180,0,0,90,self.gripper]
def __init__(self): self.sup = Supervisor() self.timeStep = int(4 * self.sup.getBasicTimeStep()) # Create the arm chain from the URDF self.armChain = Chain.from_urdf_file('ur5e.urdf') self.arm_motors = self.getARMJoint() self.grip_motors = self.getGripperJoint()
def _init_forward_kinematics(self, urdf_file): gripper_tip_elements = get_chain_from_joints( urdf_file, joints=[ 'right_arm_mount', 'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6', 'right_hand', 'right_gripper_base_joint', 'right_gripper_tip_joint' ]) self.fk_chain = Chain.from_urdf_file( urdf_file, base_elements=gripper_tip_elements, active_links_mask=[True] + 8 * [True] + 3 * [False])
def get_armchain(self) -> Chain: """ with tempfile.NamedTemporaryFile(suffix='.urdf', delete=False) as file: filename = file.name file.write(self.getUrdf().encode('utf-8')) """ """ with open("robot.urdf","w") as f: f.write(self.getUrdf()) #""" arm_chain = Chain.from_urdf_file("../robot_control/robot.urdf") return arm_chain
def get_torso_left_arm(): return Chain.from_urdf_file("./poppy_torso.URDF", base_elements=[ "base", "abs_z", "spine", "bust_y", "bust_motors", "bust_x", "chest", "l_shoulder_y" ], last_link_vector=[0, 0.18, 0], active_links_mask=[ False, False, False, False, True, True, True, True, True ], name="poppy_torso_left_arm")
def torso_right_arm(): chain1 = Chain.from_urdf_file("../resources/poppy_torso/poppy_torso.URDF", base_elements=[ "base", "abs_z", "spine", "bust_y", "bust_motors", "bust_x", "chest", "r_shoulder_y" ], last_link_vector=[0, 0.18, 0], active_links_mask=[ False, False, False, False, True, True, True, True, True ], name="poppy_torso_right_arm") return chain1
def __init__(self): rospy.init_node('kinematics') rospy.loginfo("Kinematic thingy initializin' here...") self.chain = Chain.from_urdf_file("urdf/rbx1_urdf.urdf", active_links_mask=[ False, True, True, True, True, True, True, True, False, False ]) self.publisher = rospy.Publisher('/joint_states', JointState, queue_size=10) self.seq = 0 print(self.chain)
def solve_hip_angles(v_o, v_new, a1, a2, a3, b1, b2, b3): chain = Chain( name='arm', links=[ #OriginLink(), URDFLink( name="hip1", translation_vector=[0, 0, 0], orientation=[0, 0, 0], bounds=b1, rotation=a1, ), URDFLink( name="hip2", translation_vector=[0, 0, 0], orientation=[0, 0, 0], bounds=b2, rotation=a2, ), URDFLink( name="hip3", translation_vector=[0, 0, 0], orientation=[0, 0, 0], bounds=b3, rotation=a3, ), URDFLink( name="knee", translation_vector=v_o, orientation=[0, 0, 0], rotation=[0, 0, 0], ) ]) target_frame = np.eye(4) target_frame[:3, 3] = v_new angles = chain.inverse_kinematics(target_frame)[:3] return np.rad2deg(angles)
def __init__(self, motorList, psList=None, urdf_file_path="panda_with_bound.urdf"): """ @param motorList: 各關節的驅動器。 psList: position sensor list,各關節的角度偵測器 urdf_file_path: 手臂的URDF檔案位置 注意,以上參數的關節個數必須相同 """ self.motorList = motorList self.psList = psList self.armChain = Chain.from_urdf_file(urdf_file_path) self.script = None
def __init__(self, *args, **kwargs): self.chain = Chain( name='left_arm', links=[ OriginLink(), URDFLink( name="shoulder_left", translation_vector=[0.13182, -2.77556e-17, 0.303536], orientation=[-0.752186, -0.309384, -0.752186], rotation=[0.707107, 0, 0.707107], ), URDFLink( name="proximal_left", translation_vector=[-0.0141421, 0, 0.0424264], orientation=[-7.77156e-16, 7.77156e-16, 5.55112e-17], rotation=[-0.707107, 0, 0.707107], ), URDFLink( name="distal_left", translation_vector=[0.193394, -0.0097, 0.166524], orientation=[1.66533e-16, -7.21645e-16, 3.88578e-16], rotation=[0, -1, 0], ) ])
def __init__(self): self.supervisor = Supervisor() self.timeStep = int(4 * self.supervisor.getBasicTimeStep()) # Create the arm chain from the URDF with tempfile.NamedTemporaryFile(suffix='.urdf', delete=False) as file: filename = file.name file.write(self.supervisor.getUrdf().encode('utf-8')) armChain = Chain.from_urdf_file(filename) armChain.active_links_mask[0] = False # Initialize the arm motors and encoders. self.motors = [] for link in armChain.links: if 'motor' in link.name: motor = self.supervisor.getDevice(link.name) motor.setVelocity(1.0) position_sensor = motor.getPositionSensor() position_sensor.enable(self.timeStep) self.motors.append(motor) self.arm = self.supervisor.getSelf() self.positions = [0 for _ in self.motors]
def _get_right_arm(self) -> Chain: return Chain(name="right_arm", links=[ OriginLink(), URDFLink(name="shoulder_pitch", translation_vector=[0.235, 0.565, 0.075], orientation=[-np.pi / 2, 0, 0], rotation=[-1, 0, 0], bounds=(-1.0472, 3.12414)), URDFLink(name="shoulder_yaw", translation_vector=[0, 0, 0], orientation=[0, 0, 0], rotation=[0, 1, 0], bounds=(-1.5708, 1.5708)), URDFLink(name="shoulder_roll", translation_vector=[0, 0, 0], orientation=[0, 0, 0], rotation=[0, 0, 1], bounds=(-0.785398, 0.785398)), URDFLink(name="elbow_pitch", translation_vector=[0, 0, -0.235], orientation=[0, 0, 0], rotation=[-1, 0, 0], bounds=(0, 2.79253)), URDFLink(name="wrist_roll", translation_vector=[0, 0, -0.15], orientation=[0, 0, 0], rotation=[0, 0, 1], bounds=(-1.5708, 1.5708)), URDFLink(name="wrist_pitch", translation_vector=[0, 0, 0], orientation=[0, 0, 0], rotation=[-1, 0, 0], bounds=(0, 1.5708)), URDFLink(name="mitten", translation_vector=[0, 0, -0.0625], orientation=[0, 0, 0], rotation=[0, 0, 0])])
def example_chain(): left_arm_chain = Chain(name='left_arm', links=[ OriginLink(), URDFLink( name="shoulder", translation_vector=[-10, 0, 5], orientation=[0, 1.57, 0], rotation=[0, 1, 0], ), URDFLink( name="elbow", translation_vector=[25, 0, 0], orientation=[0, 0, 0], rotation=[0, 1, 0], ), URDFLink( name="wrist", translation_vector=[22, 0, 0], orientation=[0, 0, 0], rotation=[0, 1, 0], ) ]) return left_arm_chain
# ), # URDFLink( # name="claw_elbow", # translation_vector=[8, 0, 0], # orientation=[0, -1.57, 0], # rotation=[0, 1, 0] # ), # URDFLink( # name="claw_rotate", # translation_vector=[5, 0, 0], # orientation=[0, 0, 0], # rotation=[1, 0, 0], # ), # ]) claw_chain = Chain.from_urdf_file("arm_chain.urdf") ax = plt.figure().add_subplot(111, projection="3d") # plot_chain = claw_chain.plot(claw_chain.inverse_kinematics([ # [1, 0, 0, 0], # [0, 1, 0, 0], # [0, 0, 1, 0], # [0, 0, 0, 1] # ]), ax) print( claw_chain.inverse_kinematics([[1, 0, 0, 2], [0, 1, 0, 2], [0, 0, 1, 1], [0, 0, 0, 1]])) # plt.show(plot_chain)
receiver = robot.getReceiver('receiver') receiver.enable(32) receiver.setChannel(1) # initialize motors mot = [] motNames = [ 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' ] for name in motNames: mot.append(robot.getMotor(name)) # print("Got " + name) #print(robot) my_chain = Chain.from_urdf_file("../../ur10.urdf") #sprint(my_chain.links) a = np.array([6.0, -0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 7.0]) b = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) temo = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) z = 1 T1 = np.array([[0.0,0.0,0.0,1.5],\ [0.0,0.0,0.0,1.0],\ [0.0,0.0,0.0,2.0],\ [0.0,0.0,0.0,1.0]]) tempAngles = []
def get_chain(name_templates, side): assert side in ['l', 'r'] element_names = ['body_link'] + list( map(lambda s: s.format(side), name_templates)) return Chain.from_urdf_file(DARWIN_URDF, base_elements=element_names)
import numpy import matplotlib.pyplot as plt from util.error_handling import nostderr import numpy as np import math import random import ikpy as IKPY from util import line from typing import List from ikpy.chain import Chain # Global arm configuration - IMPORTANT: wraps with nostderr() to hide command line errors. ik_py = True #boolean flag: True if ik_py, False if Kinpy with nostderr(): arm_chain = Chain.from_urdf_file("models/XArm.urdf") class Node(object): """ A node generated in a RRT graph. Args: configuration: list of joint angles in radians. Corresponds to the 6 degrees of freedom on the arm. a0-- the pan angle of the base a1-- the lift angle of the base a2-- the lift angle of the elbow a3-- the pan angle of the elbow a4-- the pan angle of the wrist (a5-- how big to open the end effector -- not programmed) Attributes: bounds: An array of float arrays listing the lower and upper bounds of each arm angle.
ep_out = dev[0][(0, 0)][1] print('') print('Exit by pressing any button on the SpaceNavigator') print('') Z_push = 0 old_Z_push = 0 R_list = [0, 0, 0] old_R_list = 0 Button_number = 0 #ロボットアームの初期設定##################################### my_chain = Chain.from_urdf_file("cybathlon_robotarm.URDF") Now_pos = my_chain.forward_kinematics([0] * 4)[:, 3][0:3] #展開後の初期姿勢を設定 ############################################################## #B3M(ロボットアーム展開)###################################### my_chain_b3m = B3m_speed_servo_lib.B3M_class() #my_chain_b3m.start_arm() ############################################################## run = True while run: try: data = dev.read(ep_in.bEndpointAddress, ep_in.bLength, 0) #Z軸のプッシュ判定 if data[0] == 1: old_Z_push = copy.deepcopy(Z_push)
from controller import Supervisor from controller import Motor from controller import Camera from controller import DistanceSensor from controller import TouchSensor import endpoint_env import preprocessing from pipe import make_pipe, close_pipe, open_write_pipe, open_read_pipe, write_to_pipe, read_from_pipe from ikpy.chain import Chain from ikpy.link import OriginLink, URDFLink from ArmUtil import ToArmCoord, PSFunc import PandaController from controller import PositionSensor armChain = Chain.from_urdf_file("panda_with_bound.urdf") supervisor = Supervisor() # create the Robot instance. env = endpoint_env.EndP_env3D(supervisor) env = preprocessing.RobotPreprocessing(env) # rl = DDPG(2, 9, [-1, 1]) # get the time step of the current world. timestep = env.timestep # Environment Reset def reset(): env.reset()
le_arm_chain = Chain(name='le_arm', active_links_mask=active_links_mask, links=[ URDFLink( name="link6", translation_vector=link6 * scale, orientation=[0, 0, 0], rotation=rotation6, bounds=bounds6 ), URDFLink( name="link5", translation_vector=link5 * scale, orientation=[0, 0, 0], rotation=rotation5, bounds=bounds5 ), URDFLink( name="link4", translation_vector=link4 * scale, orientation=[0, 0, 0], rotation=rotation4, bounds=bounds4 ), URDFLink( name="link3", translation_vector=link3 * scale, orientation=[0, 0, 0], rotation=rotation3, bounds=bounds3 ), URDFLink( name="link2", translation_vector=link2 * scale, orientation=[0, 0, 0], rotation=rotation2, bounds=bounds2 ), URDFLink( name="link1", translation_vector=link1 * scale, orientation=[0, 0, 0], rotation=rotation1, bounds=bounds1 ) ])
def __init__(self, version = 'full'): """Instanciates both a pypot.Robot and a ikpy.Chain corresponding to the version of the Poppy Gripper Arm. :param string version: Optional: Version of the robot to instanciate. 'noclamp' corresponds to a version whithout the gripper motors and clamps, and the endpoint should be placed at the end of a stick fixed to the forearm Any other parameter corresponds to a full version where every motor is enabled and the endpoint is placed along the wrist's rotation axis, at the level of the grasping area :returns: The list of angular positions """ self.version = 'full' if version != 'noclamp' else 'noclamp' sleep(0.5) # Short pause to allow the robot connections to fully load # Import URDF to build the kinematic chain if version == 'noclamp': URDFpath = URDFpath_noclamp JSONpath = JSONpath_noclamp else: URDFpath = URDFpath_full JSONpath = JSONpath_full self.robot = PoppyRightGripper(config = JSONpath) self.chain = Chain.from_urdf_file(URDFpath, active_links_mask = [False, True, True, True, True, False, False]) # Change PID of arm motors MX-28 for m in filter(lambda m: hasattr(m, 'pid'), self.robot.r_arm): m.pid = (1.9, 5, 0) if self.version != 'noclamp': # Change PID of wrist motor XL-320 self.robot.r_wrist_z.pid = (4, 2, 0) # Change PID of gripper motor XL-320 self.robot.r_gripper.pid = (8, 0, 0) # Reduce max torque to keep motor temperature low for m in self.robot.motors: m.torque_limit = 70 # Calculate the shoulder position and the total arm length to define the sphere including all the reachable space self.shoulder_origin = np.ravel( np.dot(self.chain.links[1].get_transformation_matrix(0), self.chain.links[2].get_transformation_matrix(0) )[:3, 3]) self.length = sum([l._length for l in self.chain.links[3:] ]) * 0.99 # Rough approximation of the arm's length # Define the regularization parameters corresponding to each link: factor and ideal angle reg_factor = 0.01 * np.array([0, 5, 4.5, 1.8, 1, 0, 0]) reg_angles = np.radians([0., 0., 0., 0., -80., 0., 0.]) self.regularization_parameters = (reg_factor, reg_angles) # Create a new list of motors in order to sync ikpy Links with pypot Motors along the kinematic chain. self.motors = [] motors_names = [m.name for m in self.robot.motors] nMotors = len(motors_names) if self.version == 'noclamp' else len(motors_names) - 1 # The clamp motor is ignored in full version for l in self.chain.links[1:(nMotors + 1)]: # The base link isn't taken into account try: self.motors.append(self.robot.motors[motors_names.index(l.name)]) except (ValueError): raise ValueError('Failed to find a motor named "{}". Check for discrepancies between\n{} and\n{}'.format(l.name, JSONpath, URDFpath) ) # Import the angular limits of the motors self.lower_limits = [min(m.lower_limit, m.upper_limit) for m in self.motors] self.upper_limits = [max(m.lower_limit, m.upper_limit) for m in self.motors] # Set at standard starting position self.robot.compliant = False self.robot.power_up() self.init_angles() print('Gripper arm initialized and running...')