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)
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 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 build_link(name, length):
return URDFLink(
name=name,
translation_vector=(length, 0, 0),
orientation=(0, 0, 0),
rotation=(0, 0, 1),
)
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, 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, *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 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
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)
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])])
import math
from controller import Supervisor
import matplotlib.pyplot as plt
import numpy as np
import algorithms.calculations as cal
from gym import spaces
np.set_printoptions(precision=3, suppress=True)
# Create the arm chain (robot).
armChain = Chain(
name='arm',
links=[
OriginLink(),
URDFLink(name="A motor",
bounds=[-3.1415, 3.1415],
translation_vector=[0, 0, 0.159498],
orientation=[0, 0, 0],
rotation=[0, 0, 1]),
URDFLink(name="B motor",
bounds=[-1.5708, 2.61799],
translation_vector=[0.178445, -0.122498, 0.334888],
orientation=[0, 0, 0],
rotation=[0, 1, 0]),
URDFLink(name="C motor",
bounds=[-3.1415, 1.309],
translation_vector=[-0.003447, -0.0267, 1.095594],
orientation=[0, 0, 0],
rotation=[0, 1, 0]),
URDFLink(name="D motor",
bounds=[-6.98132, 6.98132],
translation_vector=[0.340095, 0.149198, 0.174998],
from time import sleep
import math
SCALER = 10
# TODO: check length of each link could be wrong
#
arm = Chain(
name="arm",
links=[
# base servo
#
URDFLink(
name="base servo",
translation_vector=[0, 0, 0], # location
orientation=[0, 0, 0],
rotation=[0, 0, 1],
bounds=(math.radians(0), math.radians(192))
# bounds = (math.radians(-192), math.radians(0))
),
URDFLink(
name="elbow low",
translation_vector=[0, 0, 0.95 * SCALER], # location
orientation=[0, 0, 0],
rotation=[0, 1, 0],
bounds=(math.radians(120), math.radians(144))
# bounds = (math.radians(-144), math.radians(36))
),
URDFLink(
name="elbow hight",
translation_vector=[0.25 * SCALER, 0, 0], # location
orientation=[-1, -1, -1],
from ikpy.chain import Chain
from ikpy.link import OriginLink, URDFLink
from ikpy import plot_utils
import numpy as np
import math
my_chain = Chain(
name='left_arm',
links=[
# OriginLink(),
URDFLink(name="base",
translation_vector=[0, 0, 0],
orientation=[0, 0, 0.],
rotation=[0, 0, 1],
bounds=(90, 45)),
URDFLink(name="elbo",
translation_vector=[0, 0, 0.5],
orientation=[0, 0, 0],
rotation=[0, 1, 0],
bounds=(0, 1)),
URDFLink(name="wrist",
translation_vector=[0.5, 0, 0],
orientation=[0, 0, 0],
rotation=[0, 1, 0],
bounds=(0, 30)),
URDFLink(
name="EOF",
translation_vector=[0.5, 0, 0],
orientation=[0, 0, 0],
rotation=[0, 1, 0],
# bounds = (0, 0)
import matplotlib.pyplot
from ikpy.chain import Chain
from ikpy.link import OriginLink, URDFLink
from ikpy import plot_utils
import numpy as np
from mpl_toolkits.mplot3d import Axes3D
test_link = Chain(name='test_arm',
links=[
OriginLink(),
URDFLink(
name="first_link",
translation_vector=[1, 1, 0],
orientation=[0, 0, 0],
rotation=[0, 0, 1],
),
URDFLink(
name="second_link",
translation_vector=[1, 0, 0],
orientation=[0, 0, 0],
rotation=[0, 0, 1],
)
])
ax = matplotlib.pyplot.figure().add_subplot(111, projection='3d')
target_vector = [1, 0, 0]
target_frame = np.eye(4)
target_frame[:3, 3] = target_vector
target_frame
from ikpy.chain import Chain
from ikpy.link import OriginLink, URDFLink
from ikpy import plot_utils
import math
# create the arm using IKpy's chain constructor with URDF links
# TODO: check length of each link could be wrong
#
arm = Chain(name="arm", links= [
URDFLink(
name = "base servo",
translation_vector = [0, 0, 0],
orientation = [ 0, 0, 0],
rotation = [0, 0, 1],
bounds=(math.radians(0), math.radians(180))
),
URDFLink(
name = "elbow low",
translation_vector = [0, 0, 8],
orientation = [0, 0, 0],
rotation = [0, 1, 0],
bounds=(math.radians(-25), math.radians(27))
),
URDFLink(
name = "elbow hight",
translation_vector = [0, 0, 2.5],
orientation = [0, 0, 0],
rotation = [0, 1, 0],
def __init__(self, init_world_model):
self.control_world_model = init_world_model.current_world_model.control
self.arm_url = init_world_model.current_world_model.url["arm"]
self.cm_to_servo_polynomial_fitter = \
joblib.load(init_world_model.current_world_model.control["cm_to_servo_polynomial_fitter"]["file_path"])
self.init_position = np.array(self.control_world_model["init_position"]
) * self.control_world_model["scale"]
self.link_bounds = tuple(
np.radians(
np.array([
-self.control_world_model["angle_degree_limit"],
self.control_world_model["angle_degree_limit"]
]))) # In degrees
self.le_arm_chain = Chain(
name=self.control_world_model["chain_name"],
active_links_mask=self.control_world_model["active_links_mask"],
links=[
URDFLink(
name="link6",
translation_vector=np.array(
self.control_world_model["link_lengths"]["link6"]) *
self.control_world_model["scale"],
orientation=self.control_world_model["link_orientations"]
["link6"],
rotation=np.array(
self.control_world_model["joint_rotation_axis"]
["link6"]),
bounds=self.link_bounds),
URDFLink(
name="link5",
translation_vector=np.array(
self.control_world_model["link_lengths"]["link5"]) *
self.control_world_model["scale"],
orientation=self.control_world_model["link_orientations"]
["link5"],
rotation=np.array(
self.control_world_model["joint_rotation_axis"]
["link5"]),
bounds=self.link_bounds),
URDFLink(
name="link4",
translation_vector=np.array(
self.control_world_model["link_lengths"]["link4"]) *
self.control_world_model["scale"],
orientation=self.control_world_model["link_orientations"]
["link4"],
rotation=np.array(
self.control_world_model["joint_rotation_axis"]
["link4"]),
bounds=self.link_bounds),
URDFLink(
name="link3",
translation_vector=np.array(
self.control_world_model["link_lengths"]["link3"]) *
self.control_world_model["scale"],
orientation=self.control_world_model["link_orientations"]
["link3"],
rotation=np.array(
self.control_world_model["joint_rotation_axis"]
["link3"]),
bounds=self.link_bounds),
URDFLink(
name="link2",
translation_vector=np.array(
self.control_world_model["link_lengths"]["link2"]) *
self.control_world_model["scale"],
orientation=self.control_world_model["link_orientations"]
["link2"],
rotation=np.array(
self.control_world_model["joint_rotation_axis"]
["link2"]),
bounds=self.link_bounds),
URDFLink(
name="link1",
translation_vector=np.array(
self.control_world_model["link_lengths"]["link1"]) *
self.control_world_model["scale"],
orientation=self.control_world_model["link_orientations"]
["link1"],
rotation=np.array(
self.control_world_model["joint_rotation_axis"]
["link1"]),
bounds=self.link_bounds)
])
from ikpy.chain import Chain
import ikpy.geometry_utils as geometry_utils
import matplotlib.pyplot
from mpl_toolkits.mplot3d import Axes3D
from ikpy.link import OriginLink, URDFLink
import numpy as np
left_arm_chain = Chain(name='left_arm',
links=[
OriginLink(),
URDFLink(
name="shoulder",
translation_vector=[-0.59, 0, 0.44],
orientation=[0, 0, 0],
rotation=[0, 0, 0],
),
URDFLink(
name="elbow",
translation_vector=[0, 0, -0.53],
orientation=[0, 0, 0],
rotation=[0, 0, 0],
),
URDFLink(
name="wrist",
translation_vector=[0, 0, -0.74],
orientation=[0, 0, 0],
rotation=[0, 0, 0],
)
],
active_links_mask=[False, True, True, True])
armLength = 5
bodyLength = 12
bodyWidth = 10
distanceFloor = 7
stepLength = 5.0
swingHeight = 0.3
leanLeft = 0 # Not working, yet
leanRight = -leanLeft # Not working, yet
leanForward = 0 # cm
left_arm = Chain(name='left_arm',
links=[
URDFLink(
name="center",
translation_vector=[leanForward, 0, distanceFloor],
orientation=[0, 0, 0],
rotation=[0, 0, 0],
),
URDFLink(
name="shoulder",
translation_vector=[0, bodyWidth / 2, leanLeft],
orientation=[0, 0, 0],
rotation=[0, 1, 0],
bounds=(0.1 * deg2rad, 179.9 * deg2rad),
),
URDFLink(
name="upperArm",
translation_vector=[armLength, 0, 0],
orientation=[0, 0, 0],
rotation=[0, 1, 0],
bounds=(-179.9 * deg2rad, -0.1 * deg2rad),
def __init__(self):
self._done = False
self._head = baxter_interface.Head()
self._left_arm = baxter_interface.limb.Limb('left')
self._right_arm = baxter_interface.limb.Limb('right')
print(self._left_arm.joint_names())
print(self._left_arm.joint_angles())
print(self._left_arm.joint_velocities())
print(self._left_arm.joint_efforts())
print(self._left_arm.endpoint_pose())
print(self._left_arm.endpoint_velocity())
print(self._left_arm.endpoint_effort())
self._left_arm_chain = Chain(
name='left_arm',
active_links_mask=[
False, False, True, True, True, True, True, True, True, False
],
links=[
OriginLink(),
URDFLink(
name="left_torso_arm_mount",
translation_vector=[0.024645, 0.219645, 0.118588],
orientation=[0, 0, 0.7854],
rotation=[0, 0, 1],
),
URDFLink(name="left_s0",
translation_vector=[0.055695, 0, 0.011038],
orientation=[0, 0, 0],
rotation=[0, 0, 1],
bounds=(-1.70167993878, 1.70167993878)),
URDFLink(name="left_s1",
translation_vector=[0.069, 0, 0.27035],
orientation=[-1.57079632679, 0, 0],
rotation=[0, 0, 1],
bounds=(-2.147, 1.047)),
URDFLink(name="left_e0",
translation_vector=[0.102, 0, 0],
orientation=[1.57079632679, 0, 1.57079632679],
rotation=[0, 0, 1],
bounds=(-3.05417993878, 3.05417993878)),
URDFLink(name="left_e1",
translation_vector=[0.069, 0, 0.26242],
orientation=[-1.57079632679, -1.57079632679, 0],
rotation=[0, 0, 1],
bounds=(-0.05, 2.618)),
URDFLink(name="left_w0",
translation_vector=[0.10359, 0, 0],
orientation=[1.57079632679, 0, 1.57079632679],
rotation=[0, 0, 1],
bounds=(-3.059, 3.059)),
URDFLink(name="left_w1",
translation_vector=[0.01, 0, 0.2707],
orientation=[-1.57079632679, -1.57079632679, 0],
rotation=[0, 0, 1],
bounds=(-1.57079632679, 2.094)),
URDFLink(name="left_w2",
translation_vector=[0.115975, 0, 0],
orientation=[1.57079632679, 0, 1.57079632679],
rotation=[0, 0, 1],
bounds=(-3.059, 3.059)),
URDFLink(
name="left_hand",
translation_vector=[0, 0, 0.11355],
orientation=[0, 0, 0],
rotation=[0, 0, 1],
)
])
#self._left_arm_chain = ikpy.chain.Chain.from_urdf_file("/home/jbunker/ros_ws/src/baxter_common/baxter_description/urdf/baxter.urdf")
# verify robot is enabled
print("Getting robot state... ")
self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
print("Running. Ctrl-c to quit")
s1 = 90 - settheta[1] - 120 - t2
settheta[1] = settheta[1] + 120
x1 = np.sqrt(a**2 + 129**2 - 2 * a * 129 * np.cos(s1))
x2 = np.arccos((a**2 + x1**2 - 129**2) / (2 * a * x1))
x3 = np.arccos((x1**2 + 83**2 - 65**2) / (2 * 83 * x1))
settheta[3] = 180 - (360 - s1 - s2 - x2 - x3) - 90
print(settheta)
(theta1, theta2, theta3, theta4) = (0, 0, 0, 0)
mychain = Chain(
name='neck',
links=[
#OriginLink(),
URDFLink(
name="shoulder",
translation_vector=[0, 0, 0.065],
orientation=[-0.5 * PI, 0, 0],
rotation=[0, 0, 1],
)
# URDFLink(
# name="elbow",
# translation_vector=[0, 0, 0.129],
# orientation=[-PI*(156/180.0)+theta2, 0, 0],
# rotation=[0, 0, 1],
# ),
# URDFLink(
# name="wrist",
# translation_vector=[0, 0, 0.065],
# orientation=[(99/180.0)*PI+theta3, 0, 0],
# rotation=[0, 0, 1],
# ),
# URDFLink(
link3 = np.array([0, 0.0, 0.9])
# link4 = np.array([0, 0.0, 6.0])
# link4 = np.array([0, 0.0, 0.06])
link4 = np.array([0, 0.0, 0.6])
# link5 = np.array([0, 0.0, 12.0])
# link5 = np.array([0, 0.0, 0.12])
link5 = np.array([0, 0.0, 1.2])
robotic_arm_chain = Chain(
name='robotic_arm',
links=[
# OriginLink(),
URDFLink(
name="base",
# translation_vector=[-10, 0, 5],
translation_vector=base,
# orientation=[0, 1.57, 0],
orientation=[0, 0, 0],
rotation=[0, 0, 1],
),
URDFLink(
name="link1",
# translation_vector=[-10, 0, 5],
translation_vector=link1,
# orientation=[0, 1.57, 0],
orientation=[0, 0, 0],
rotation=[0, 0, 1],
),
URDFLink(
name="link2",
# translation_vector=[25, 0, 0],
translation_vector=link2,
from ikpy.chain import Chain
from ikpy.link import OriginLink, URDFLink
import ikpy.utils.plot as plot_utils
import matplotlib.pyplot as plt
import numpy as np
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],
)
])
fig, ax = plot_utils.init_3d_figure()
left_arm_chain.plot([0, 0, 0, 0], ax)
from ikpy.chain import Chain
from ikpy.link import OriginLink, URDFLink
my_chain = Chain(name='cybathlon', links=[
URDFLink(
name="joint1",
translation_vector=[0,0,0],
orientation=[0, 0, 0],
rotation=[0, 1, 0],
),
URDFLink(
name="joint2",
translation_vector=[0,0,0.075],
orientation=[0, 0, 0],
rotation=[1, 0, 0],
),
URDFLink(
name="joint3",
translation_vector=[0.045, 0, 0.802],
orientation=[0, 0, 0],
rotation=[0, 1, 0],
),
URDFLink(
name="joint4",
translation_vector=[-0.045, 0, 0.746],
orientation=[0, 0, 0],
rotation=[0, 0, 0],
)
])
print("start")
rotation2 = np.array([0, 0, 1])
rotation1 = np.array([0, 0, 1])
# Link bounds (degrees) # TODO: per servo bounds
bounds6 = np.radians(np.array([-angle_degree_limit, angle_degree_limit]))
bounds5 = np.radians(np.array([-angle_degree_limit, angle_degree_limit]))
bounds4 = np.radians(np.array([-angle_degree_limit, angle_degree_limit]))
bounds3 = np.radians(np.array([-angle_degree_limit, angle_degree_limit]))
bounds2 = np.radians(np.array([-angle_degree_limit, angle_degree_limit]))
bounds1 = np.radians(np.array([-angle_degree_limit, angle_degree_limit]))
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,
from ikpy.chain import Chain
from ikpy.link import OriginLink, URDFLink
import math
from controller import *
supervisor = Supervisor()
timestep = int(4)
RArmChain = Chain(name='RArm',
links=[
OriginLink(),
URDFLink(
name="RArmUsy",
bounds=[-1.9635, 1.9635],
translation_vector=[0.024, -0.221, 0.289],
orientation=[0, 0, 0],
rotation=[0, 0.5, -0.866025],
),
URDFLink(
name="RArmShx",
bounds=[-1.74533, 1.39626],
translation_vector=[0, -0.075, 0.036],
orientation=[0, 0, 0],
rotation=[1, 0, 0],
),
URDFLink(name="RArmEly",
bounds=[0, 3.14159],
translation_vector=[0, -0.185, 0],
orientation=[0, 0, 0],
rotation=[0, 1, 0]),
