import ikpy
import numpy as np
from ikpy import plot_utils
from ikpy.chain import Chain
from ikpy.link import OriginLink, URDFLink
my_chain = Chain(name='left_arm',
links=[
OriginLink(),
URDFLink(
name="shoulder",
translation_vector=[-0.1, 0, 0.5],
orientation=[0, 1.57, 0],
rotation=[0, 1, 0],
),
URDFLink(
name="elbow",
translation_vector=[0.1, 0, 0],
orientation=[0, 0, 0],
rotation=[0, 1, 0],
),
URDFLink(
name="wrist",
translation_vector=[0.1, 0, 0],
orientation=[0, 0, 0],
rotation=[0, 1, 0],
)
])
#my_chain = ikpy.chain.Chain.from_urdf_file("ikpy/resources/poppy_ergo.URDF")
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")
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
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]),