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)
