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): 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 __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): 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]
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()
#!/usr/bin/env python import ikpy from ikpy.chain import Chain import numpy as np import ikpy.utils.plot as plot_utils my_chain = Chain.from_urdf_file( "/home/pi/Desktop/ikpy-master/resources/poppy_ergo.URDF") target_position = [0.1, -0.2, 0.1] print("The angles of each joints are : ", my_chain.inverse_kinematics(target_position)) real_frame = my_chain.forward_kinematics( my_chain.inverse_kinematics(target_position)) print("Computed position vector : %s, original position vector : %s" % (real_frame[:3, 3], target_frame[:3, 3])) # Optional: support for 3D plotting in the NB # If there is a matplotlib error, uncomment the next line, and comment the line below it. #%matplotlib inline #%matplotlib widget import matplotlib.pyplot as plt fig, ax = plot_utils.init_3d_figure() my_chain.plot(my_chain.inverse_kinematics(target_position), ax, target=target_vector) plt.xlim(-0.1, 0.1) plt.ylim(-0.1, 0.1)
# ), # 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)
'The "ikpy" Python module version is too old. ' 'Please upgrade "ikpy" Python module to version "3.0" or newer with this command: "pip install --upgrade ikpy"' ) IKPY_MAX_ITERATIONS = 4 # Initialize the Webots Supervisor. supervisor = Supervisor() timeStep = int(4 * supervisor.getBasicTimeStep()) # Create the arm chain from the URDF filename = None with tempfile.NamedTemporaryFile(suffix='.urdf', delete=False) as file: filename = file.name file.write(supervisor.getUrdf().encode('utf-8')) armChain = Chain.from_urdf_file(filename) for i in [0, 6]: armChain.active_links_mask[0] = False # Initialize the arm motors and encoders. motors = [] for link in armChain.links: if 'motor' in link.name: motor = supervisor.getDevice(link.name) motor.setVelocity(1.0) position_sensor = motor.getPositionSensor() position_sensor.enable(timeStep) motors.append(motor) # Get the arm and target nodes. target = supervisor.getFromDef('TARGET')
from ikpy.chain import Chain import numpy as np import matplotlib.pyplot from mpl_toolkits.mplot3d import Axes3D active_links = [False, True, False, True, True, True, False, False] arm_chain = Chain.from_urdf_file("arm_gazebo.urdf", base_elements=["base_link"], last_link_vector=np.array([0, 0, 0.045]), active_links_mask=active_links) print(arm_chain) transformation_mat = arm_chain.forward_kinematics(np.ones(8)) end_pos = transformation_mat[:3, 3] print(end_pos) arm_pos = np.array([0, 0, 45, 45]) ax = matplotlib.pyplot.figure().add_subplot(111, projection='3d') arm_chain.plot( np.array([0, arm_pos[0], 0, arm_pos[1], arm_pos[2], arm_pos[3], 0, 0]), ax) matplotlib.pyplot.show() arm_chain.plot()
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...')
def __init__(self, robot, tip): self._chain = Chain.from_urdf_file(robot.urdf_file, base_elements=['base'], last_link_vector=tip) self._robot = robot
def _get_ikpy_chain(base_link): # type: (str) -> Chain robot_description: str = rospy.get_param('robot_description') with TemporaryFile() as file: file.write(robot_description.encode()) file.seek(0) return Chain.from_urdf_file(file, [base_link])
# link name="clash_hand__right_proximal # link name="clash_hand__right_distal # link name="clash_hand__thumb_base # link name="clash_hand__thumb_proximal # link name="clash_hand__thumb_distal # joint name="clash_hand__left_body_proximal" type="revolute # joint name="clash_hand__left_proximal_distal" type="revolute # joint name="clash_hand__right_body_proximal" type="revolute # joint name="clash_hand__right_proximal_distal" type="revolute # joint name="clash_hand__thumb_body_base" type="revolute # joint name="clash_hand__thumb_base_proximal" type="revolute # joint name="clash_hand__thumb_proximal_distal" type="revolute urdf = os.getenv("mod_handconvert_root") + '/adaptedURDF/model.urdf' leftChain = Chain.from_urdf_file( urdf, name='left finger', base_elements=['clash_hand__body', 'clash_hand__left_body_proximal']) rightChain = Chain.from_urdf_file( urdf, name='right finger', base_elements=['clash_hand__body', 'clash_hand__right_body_proximal']) thumbChain = Chain.from_urdf_file( urdf, name='thumb', base_elements=['clash_hand__body', 'clash_hand__thumb_body_base']) # chain = Chain.from_urdf_file(urdf, # base_elements=['clash_hand__body', 'clash_hand__thumb_base'], # name='clash_urdf') # chain = Chain.from_urdf_file(urdf, name='clash_urdf')
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)
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 = []
from ikpy.chain import Chain from ikpy.link import OriginLink, URDFLink import matplotlib.pyplot from mpl_toolkits.mplot3d import Axes3D ax = matplotlib.pyplot.figure().add_subplot(111, projection='3d') #my_chain = Chain.from_urdf_file("test_robot.URDF") # my_chain = Chain.from_urdf_file("cybathlon_robotarm.URDF") my_chain = Chain.from_urdf_file("urdf/model.URDF") print("start") #my_chain.plot(my_chain.inverse_kinematics([ # [1, 0, 0, 5], # [0, 1, 0, 0], # [0, 0, 1, -5], # [0, 0, 0, 1] # ]),ax) #matplotlib.pyplot.show() #print(my_chain.inverse_kinematics([ # [1, 0, 0, 5], # [0, 1, 0, 0], # [0, 0, 1, -5], # [0, 0, 0, 1] # ])) my_chain.plot([0, 0, 0, 0], ax) pos = my_chain.forward_kinematics([0] * 4)[:, 3][0:3] print(pos) #matplotlib.pyplot.show() matplotlib.pyplot.pause(1)
# You should insert a getDevice-like function in order to get the # instance of a device of the robot. Something like: # motor = robot.getMotor('motorname') # ds = robot.getDistanceSensor('dsname') # ds.enable(timestep) # with open("tiago_urdf.urdf", "w") as file: # file.write(robot.getUrdf()) code = "20982" base_elements=["base_link", "base_link_Torso_joint", "Torso", "torso_lift_joint", "torso_lift_link", "torso_lift_link_TIAGo front arm_" + code + "_joint", "TIAGo front arm_" + code] filename = "tiago_urdf.urdf" my_chain = Chain.from_urdf_file(filename, base_elements=base_elements) print(my_chain.links) vector =[0.003999353, 0, -0.1741] my_chain = Chain.from_urdf_file(filename, last_link_vector=vector, base_elements=base_elements) print(my_chain.links) hands = ("gripper_left_finger_joint", "gripper_right_finger_joint") part_names = ("head_2_joint", "head_1_joint", "torso_lift_joint", "arm_1_joint", "arm_2_joint", "arm_3_joint", "arm_4_joint", "arm_5_joint", "arm_6_joint", "arm_7_joint", "wheel_left_joint", "wheel_right_joint")
def getChain(): urdfPath = "RescueBot_Baugruppe.SLDASM/urdf/RescueBot_Baugruppe.SLDASM.urdf" return Chain.from_urdf_file(urdfPath)