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]),