def test_graphical_robot_creation():
"""
This test will create a simple 3-link graphical robot.
The joints are then set to new poses to show updating does occur.
"""
p = SE3()
p1 = p
p2 = p.Tx(1)
p3 = p.Tx(2)
robot = gph.GraphicalRobot(g_canvas, 'test_3_link_robot')
robot.append_link('r', p1, 1.0)
robot.append_link('R', p2, 1.0)
robot.append_link('r', p3, 1.0)
arr = array([[0, 0, 1, 0], [0, 1, 0, 0], [1, 0, 0, 0], [0, 0, 0, 1]])
new_p1 = SE3(arr)
arr = array([[0, 0, 1, 0], [0, 1, 0, 0], [1, 0, 0, 1], [0, 0, 0, 1]])
new_p2 = SE3(arr)
arr = array([[0, 0, 1, 0], [0, 1, 0, 0], [1, 0, 0, 2], [0, 0, 0, 1]])
new_p3 = SE3(arr)
sleep(2)
robot.set_joint_poses([new_p1, new_p2, new_p3])
def test_multiple_robots():
p = SE3()
p1 = p
p2 = p.Tx(1)
p3 = p.Tx(2)
robot = gph.GraphicalRobot(g_canvas, 'Robot A')
robot.append_link('r', p1, 1.0)
robot.append_link('R', p2, 1.0)
robot.append_link('r', p3, 1.0)
arr = array([[0, 0, 1, 0], [0, 1, 0, 0], [1, 0, 0, 0], [0, 0, 0, 1]])
new_p1 = SE3(arr)
arr = array([[0, 0, 1, 0], [0, 1, 0, 0], [1, 0, 0, 1], [0, 0, 0, 1]])
new_p2 = SE3(arr)
arr = array([[0, 0, 1, 0], [0, 1, 0, 0], [1, 0, 0, 2], [0, 0, 0, 1]])
new_p3 = SE3(arr)
robot2 = gph.GraphicalRobot(g_canvas, 'Robot B')
robot2.append_link('r', new_p1, 1.0)
robot2.append_link('R', new_p2, 1.0)
robot2.append_link('r', new_p3, 1.0)
def exp(self, theta=None, units='rad'):
"""
Exponentiate a twist
:param theta: DESCRIPTION, defaults to None
:type theta: TYPE, optional
:param units: DESCRIPTION, defaults to 'rad'
:type units: TYPE, optional
:return: DESCRIPTION
:rtype: TYPE
TW.exp is the homogeneous transformation equivalent to the twist (SE2 or SE3).
TW.exp(THETA) as above but with a rotation of THETA about the twist.
Notes::
- For the second form the twist must, if rotational, have a unit rotational component.
See also Twist.T, trexp, trexp2.
"""
if units != 'rad' and self.isprismatic:
print('Twist.exp: using degree mode for a prismatic twist')
if theta is None:
theta = 1
else:
theta = argcheck.getunit(theta, units)
if isinstance(theta, (int, np.int64, float, np.float64)):
return SE3(tr.trexp(self.S * theta))
else:
return SE3([tr.trexp(self.S * t) for t in theta])
def _get_initial_qpos(self):
"""
Calculates the initial joint position for the robot based on the initial desired pose (self.traj_pt, self.goal_quat).
Args:
Returns:
(np.array): n joint positions
"""
pos = self._convert_robosuite_to_toolbox_xpos(self.traj_pt)
ori_euler = mat2euler(quat2mat(self.goal_quat))
# desired pose
T = SE3(pos) * SE3.RPY(ori_euler)
# find initial joint positions
if self.robots[0].name == "UR5e":
robot = rtb.models.DH.UR5()
sol = robot.ikine_min(T, q0=self.robots[0].init_qpos)
# flip last joint around (pi)
sol.q[-1] -= np.pi
return sol.q
elif self.robots[0].name == "Panda":
robot = rtb.models.DH.Panda()
sol = robot.ikine_min(T, q0=self.robots[0].init_qpos)
return sol.q
def __init__(self, graphics_canvas, name, robot=None):
self.joints = []
self.num_joints = 0
self.rob_shown = True
self.ref_shown = True
self.opacity = 1
self.name = name
self.__scene = graphics_canvas
self.angles = []
self.robot = robot
# If Robot given, create the robot
if self.robot is not None:
# Update name
self.name = self.robot.name
# Get initial poses
zero_angles = self.robot.qz
all_poses = self.robot.fkine_all(zero_angles)
# Create the base
if robot.basemesh is not None:
self.append_link("s", robot.base, robot.basemesh, [0, 0], 0)
# else: assume no base joint
# Create the joints
i = 0
for link in self.robot.links:
# Get info
if link.isprismatic():
j_type = 'p'
elif link.isrevolute():
j_type = 'r'
else:
j_type = 's'
pose = all_poses[i] # Pose
if link.mesh is None:
if i == 0:
x1, x2 = SE3().t[0], all_poses[i].t[0]
y1, y2 = SE3().t[1], all_poses[i].t[1]
z1, z2 = SE3().t[2], all_poses[i].t[2]
length = sqrt(
(x2 - x1) * (x2 - x1) + (y2 - y1)
* (y2 - y1) + (z2 - z1) * (z2 - z1)) # Length
else:
x1, x2 = all_poses[i - 1].t[0], all_poses[i].t[0]
y1, y2 = all_poses[i - 1].t[1], all_poses[i].t[1]
z1, z2 = all_poses[i - 1].t[2], all_poses[i].t[2]
length = sqrt(
(x2 - x1) * (x2 - x1) + (y2 - y1)
* (y2 - y1) + (z2 - z1) * (z2 - z1)) # Length
else:
length = link.mesh
angle_lims = link.qlim # Angle limits
theta = link.theta # Current angle
i += 1
self.append_link(j_type, pose, length, angle_lims, theta)
# Add the robot to the canvas UI
graphics_canvas.add_robot(self)
def base(self, T):
# if not isinstance(T, SE3):
# T = SE3(T)
if T is None or isinstance(T, SE3):
self._base = T
elif SE3.isvalid(T):
self._tool = SE3(T, check=False)
else:
raise ValueError('base must be set to None (no tool) or an SE3')
def setUp(self):
self.scene = canvas.GraphicsCanvas3D()
# 0.707107 -0.707107 0 0
# 0.707107 0.707107 0 1
# 0 0 1 0.4
# 0 0 0 1
self.se3 = SE3().Ty(1) * SE3().Tz(0.4) * SE3().Rz(45, 'deg')
self.structure = 1.0
def __init__(self,
name=None,
camtype=None,
rho=10e-6,
imagesize=(1024, 1024),
pose=None):
"""
Create instance of a Camera class
"""
if name is None:
self._name = 'MVTB camera'
else:
if not isinstance(name, str):
raise TypeError(name, 'name must be a string')
self._name = name
if camtype is None:
self._camtype = 'perspective'
else:
if not isinstance(camtype, str):
raise TypeError(camtype, 'camtype must be a string')
self._camtype = camtype
rho = base.getvector(rho)
if len(rho) == 1:
self._rhou = rho[0]
self._rhov = rho[0]
elif len(rho) == 2:
self._rhou = rho[0]
self._rhov = rho[1]
else:
raise ValueError(rho, 'rho must be a 1- or 2-element vector')
imagesize = base.getvector(imagesize)
if len(imagesize) == 1:
self._nu = imagesize
self._nv = imagesize
elif len(imagesize) == 2:
self._nu = imagesize[0]
self._nv = imagesize[1]
else:
raise ValueError(
imagesize, 'imagesize must be a 1- or 2-element vector')
if pose is None:
self._pose = SE3()
else:
self._pose = SE3(pose)
self._image = None
self._fig = None
self._ax = None
self._noise = None
self._distortion = None
def go_down(connection):
servo_position = get_position(connection)
time.sleep(1)
N1 = int(servo_position[0])
head_position = get_head_position(connection, servo_pos=servo_position)
rbot = rtb.models.DH.widowx()
pos = [
head_position[0], 0, head_position[1] + 36
] # [x,y,z] = [l, 0, 36+h], 0 mais ballec faut juste pas commander le servo 1
pas = 20
dt = 0.31
H = np.flip(np.append(np.arange(-50, pos[2], pas), pos[2]))
viapt = []
ang_reel = [
servo_position[0] * (pi / 2048),
(servo_position[1] - 1024) * (pi / 2048),
(servo_position[2] - 1024) * (pi / 2048)
] # [theta_1, theta_2, theta_3], angles réels du robot
ang = [ang_reel[0], pi - 0.3443 - ang_reel[1], ang_reel[2] + 0.3443 - pi
] # [phi_1, phi_2, phi_3], angles utilisés par le modèle
for h in H:
T = SE3(pos[0], pos[1], h) * SE3.OA([0, 1, 0], [0, 0, -1])
sol = rbot.ikinem(T, q0=np.array(
([ang[0]], [ang[1]],
[ang[2]])).T) # ikinem: cinématique inverse numérique,
viapt.append(sol[0])
ang = [sol[0][0], sol[0][1], sol[0][2]]
VIA = np.array(viapt)
qt = rtb.trajectory.mstraj(
VIA, dt, tacc=0.2, qdmax=10
) # tacc : temps d'accélération | qdmax : vitesse max | dt : delay
# rbot.plot(qt.q, movie='panda1.gif') # plot de la trajectoire
traj = qt.q
print("trajectoire :" + str(len(traj)))
for i in range(len(traj)):
ang2_mod = traj[i][1]
ang3_mod = traj[i][2]
ang2_reel = pi - 0.3443 - ang2_mod
ang3_reel = ang3_mod - 0.3443 + pi
N2_pile = (2048 / pi) * ang2_reel + 1024
N3_pile = (2048 / pi) * ang3_reel + 1024
N2 = int((2048 / pi) * ang2_reel + 1024)
N3 = int((2048 / pi) * ang3_reel + 1024)
N4 = int(1024 + N2_pile - N3_pile)
def tool(self, T):
# if not isinstance(T, SE3):
# T = SE3(T)
# this is allowed to be none, it's helpful for symbolics rather than having an identity matrix
if T is None or isinstance(T, SE3):
self._tool = T
elif SE3.isvalid(T):
self._tool = SE3(T, check=False)
else:
raise ValueError('tool must be set to None (no tool) or an SE3')
def setUp(self):
self.robot_scene = GraphicsCanvas3D()
# 0.707107 -0.707107 0 0
# 0.707107 0.707107 0 1
# 0 0 1 0.4
# 0 0 0 1
self.robot_se3 = SE3().Ty(1) * SE3().Tz(0.4) * SE3().Rz(45, 'deg')
self.robot_structure = 1.0
self.se3 = SE3().Tx(3)
warnings.simplefilter('ignore', category=ResourceWarning)
def test_multiply(self):
T1 = SE3.Rx(pi / 4)
T2 = SE3.Rz(-pi / 3)
T = T1 * T2
d1 = UnitDualQuaternion(T1)
d2 = UnitDualQuaternion(T2)
d = d1 * d2
nt.assert_array_almost_equal(d.SE3().A, T.A)
def rne(robot, q, qd, qdd, gravity=None):
n = robot.n
# allocate intermediate variables
Xup = SE3.Alloc(n)
Xtree = SE3.Alloc(n)
v = SpatialVelocity.Alloc(n)
a = SpatialAcceleration.Alloc(n)
f = SpatialForce.Alloc(n)
I = SpatialInertia.Alloc(n) # noqa
s = [None for i in range(n)] # joint motion subspace
Q = np.zeros((n, )) # joint torque/force
# initialize intermediate variables
for j, link in enumerate(robot):
I[j] = SpatialInertia(m=link.m, r=link.r)
Xtree[j] = link.Ts
s[j] = link.v.s
if gravity is None:
a_grav = SpatialAcceleration(robot.gravity)
else:
a_grav = SpatialAcceleration(gravity)
# forward recursion
for j in range(0, n):
vJ = SpatialVelocity(s[j] * qd[j])
# transform from parent(j) to j
Xup[j] = robot[j].A(q[j]).inv()
if robot[j].parent is None:
v[j] = vJ
a[j] = Xup[j] * a_grav + SpatialAcceleration(s[j] * qdd[j])
else:
jp = robot[j].parent.jindex
v[j] = Xup[j] * v[jp] + vJ
a[j] = Xup[j] * a[jp] \
+ SpatialAcceleration(s[j] * qdd[j]) \
+ v[j] @ vJ
f[j] = I[j] * a[j] + v[j] @ (I[j] * v[j])
# backward recursion
for j in reversed(range(0, n)):
Q[j] = f[j].dot(s[j])
if robot[j].parent is not None:
jp = robot[j].parent.jindex
f[jp] = f[jp] + Xup[j] * f[j]
return Q
def p_servo(wTe, wTep, gain=2, threshold=0.1):
'''
Position-based servoing.
Returns the end-effector velocity which will cause the robot to approach
the desired pose.
:param wTe: The current pose of the end-effecor in the base frame.
:type wTe: SE3
:param wTep: The desired pose of the end-effecor in the base frame.
:type wTep: SE3
:param gain: The gain for the controller
:type gain: float
:param threshold: The threshold or tolerance of the final error between
the robot's pose and desired pose
:type threshold: float
:returns v: The velocity of the end-effecotr which will casue the robot
to approach wTep
:rtype v: ndarray(6)
:returns arrived: True if the robot is within the threshold of the final
pose
:rtype arrived: bool
'''
if not isinstance(wTe, SE3):
wTe = SE3(wTe)
if not isinstance(wTep, SE3):
wTep = SE3(wTep)
# Pose difference
eTep = wTe.inv() * wTep
# Translational velocity error
ev = eTep.t
# Angular velocity error
ew = eTep.rpy() * np.pi / 180
# Form error vector
e = np.r_[ev, ew]
# Desired end-effector velocity
v = gain * e
if np.sum(np.abs(e)) < threshold:
arrived = True
else:
arrived = False
return v, arrived
def test_set_joint_pose(self):
# Create a scene
self.scene.scene.title = "Test Set Joint Pose"
# Create a joint
joint = robot.RotationalJoint(self.se3, self.structure, self.scene)
# Move joint x+30d, y, z-2
joint.update_pose(self.se3 * SE3().Rx(30, 'deg') * SE3().Tz(-2))
# Check position
self.check_obj_pose(joint, self.se3 * SE3().Rx(30, 'deg') * SE3().Tz(-2))
def test_set_joint_orientation(self):
# Create a scene
self.scene.scene.title = "Test Set Joint Orientation"
# Create a joint
joint = robot.RotationalJoint(self.se3, self.structure, self.scene)
# Rotate joint x+30d, y, z+45d
joint.update_orientation(self.se3 * SE3().Rx(30, 'deg') * SE3().Rz(45, 'deg'))
# Check position
self.check_obj_pose(joint, self.se3 * SE3().Rx(30, 'deg') * SE3().Rz(45, 'deg'))
def setUp(self):
self.env.launch()
# Get the last scene
self.robot_scene = self.env.canvases[len(self.env.canvases) - 1]
# 0.707107 -0.707107 0 0
# 0.707107 0.707107 0 1
# 0 0 1 0.4
# 0 0 0 1
self.robot_se3 = SE3().Ty(1) * SE3().Tz(0.4) * SE3().Rz(45, 'deg')
self.robot_structure = 1.0
self.se3 = SE3().Tx(3)
warnings.simplefilter('ignore', category=ResourceWarning)
def __init__(self, base=None):
mm = 1e-3
deg = pi / 180
# details from
h = 53.0 * mm
r = 30.309 * mm
l2 = 170.384 * mm
l3 = 136.307 * mm
l4 = 86.00 * mm
c = 40.0 * mm
# tool_offset = l4 + c
# Turret, Shoulder, Elbow, Wrist, Claw
links = [
RevoluteDH(d=h, a=0, alpha=90 * deg), # Turret
RevoluteDH(
d=0,
a=l2,
alpha=0, # Shoulder
qlim=[10 * deg, 122.5 * deg]),
RevoluteDH(
d=0,
a=-l3,
alpha=0, # Elbow
qlim=[20 * deg, 340 * deg]),
RevoluteDH(
d=0,
a=l4 + c,
alpha=0, # Wrist
qlim=[45 * deg, 315 * deg])
]
super().__init__(links,
name="Orion 5",
manufacturer="RAWR Robotics",
base=SE3(r, 0, 0),
tool=SE3.Ry(90, 'deg'))
# zero angles, all folded up
self.addconfiguration("qz", np.r_[0, 90, 180, 180] * deg)
# stretched out vertically
self.addconfiguration("qv", np.r_[0, 90, 180, 180] * deg)
# arm horizontal, hand down
self.addconfiguration("qh", np.r_[0, 0, 180, 90] * deg)
def test_listpowers(self):
R = SE3()
R1 = SE3.Rx(0.2)
R2 = SE3.Ry(0.3)
R.append(R1)
R.append(R2)
nt.assert_equal(len(R), 3)
self.assertIsInstance(R, SE3)
array_compare(R[0], np.eye(4))
array_compare(R[1], R1)
array_compare(R[2], R2)
R = SE3([trotx(0.1), trotx(0.2), trotx(0.3)])
nt.assert_equal(len(R), 3)
self.assertIsInstance(R, SE3)
array_compare(R[0], trotx(0.1))
array_compare(R[1], trotx(0.2))
array_compare(R[2], trotx(0.3))
R = SE3([SE3.Rx(0.1), SE3.Rx(0.2), SE3.Rx(0.3)])
nt.assert_equal(len(R), 3)
self.assertIsInstance(R, SE3)
array_compare(R[0], trotx(0.1))
array_compare(R[1], trotx(0.2))
array_compare(R[2], trotx(0.3))
def A(self, q=0.0, fast=False):
"""
Link transform matrix
:param q: Joint coordinate (radians or metres). Not required for links
with no variable
:type q: float
:param fast: return NumPy array instead of ``SE3``
:type param: bool
:return T: link frame transformation matrix
:rtype T: SE3 or ndarray(4,4)
``LINK.A(q)`` is an SE(3) matrix that describes the rigid-body
transformation from the previous to the current link frame to
the next, which depends on the joint coordinate ``q``.
If ``fast`` is True return a NumPy array, either SE(2) or SE(3).
A value of None means that it is the identity matrix.
If ``fast`` is False return an ``SE2`` or ``SE3`` instance.
"""
# Use c extension
if fast:
if not np.isscalar(q):
q = 0.0
T = np.empty((4, 4))
fknm.link_A(q, self._fknm, T)
return T
# Otherwise use Python implementation
if self.isjoint:
# a variable joint
if q is None:
raise ValueError("q is required for variable joints")
# premultiply variable part by constant part if present
Ts = self.Ts
if Ts is None:
T = self.v.T(q)
else:
T = Ts @ self.v.T(q)
else:
# a fixed joint
T = self.Ts
if fast:
return T
if T is None:
return SE3()
else:
return SE3(T, check=False)
def ikine_global(
self, T, qlim=False, ilimit=1000,
tol=1e-16, method=None, options={}):
r"""
.. warning:: Experimental code for using SciPy global optimizers.
Each global optimizer has quite a different call signature, so final
design will need a bit of thought.
"""
# basinhopping:
# brute: ranges, finish=None
# differential_evolution: bounds, tol
# shgo: bounds, options:f_tol
# dual_annealing: bounds
if not isinstance(T, SE3):
T = SE3(T)
solutions = []
# wr = 1 / self.reach
# weight = np.r_[wr, wr, wr, 1, 1, 1]
optdict = {}
if method is None:
method = 'differential-evolution'
if method == 'brute':
# requires a tuple of tuples
optdict['ranges'] = tuple([tuple(li.qlim) for li in self])
else:
optdict['bounds'] = tuple([tuple(li.qlim) for li in self])
if method not in ['basinhopping', 'brute', 'differential_evolution',
'shgo', 'dual_annealing']:
raise ValueError('unknown global optimizer requested')
global_minimizer = opt.__dict__[method]
def cost(q, T, weight):
# T, weight, costfun, stiffness = args
e = _angle_axis(self.fkine(q).A, T) * weight
return (e**2).sum()
for _ in T:
res = global_minimizer(
cost,
**optdict)
solution = iksol(res.x, res.success, res.message, res.nit, res.fun)
solutions.append(solution)
q0 = res.x # use this solution as initial estimate for next time
if len(T) == 1:
return solutions[0]
else:
return solutions
def test_robot_decor():
"""
Test importing textures and changing joint colours
"""
new_rot = gph.RotationalJoint(SE3(), 1.0, g_canvas.scene)
# Load a sample texture
new_rot.set_texture(
texture_link=
"https://s3.amazonaws.com/glowscript/textures/flower_texture.jpg")
sleep(5)
# Green shift the texture
new_rot.set_texture(
colour=[0, 0.75, 0],
texture_link=
"https://s3.amazonaws.com/glowscript/textures/flower_texture.jpg")
sleep(5)
# Remove the texture and red shift
new_rot.set_texture(colour=[1, 0, 0])
sleep(5)
# Remove all details
new_rot.set_texture()
# Set transparency
new_rot.set_transparency(0.3)
def test_clear_scene_with_grid_updating():
"""
This test will import the Puma560 model, then after 2 seconds, clear the canvas of all models.
"""
puma560 = gph.import_puma_560(g_canvas.scene)
# Get the poses for a ready-position
puma = Puma560()
poses = puma.fkine(puma.config('qr'), alltout=True)
sleep(2)
puma560.set_joint_poses([
SE3(), # 0 (Base doesn't change)
poses[0], # 1
poses[1], # 2
poses[2], # 3
poses[3], # 4
poses[4], # 5
poses[5] # 6
])
sleep(2)
clear()
del puma560
def import_puma_560(g_canvas):
"""
Create a Robot class object based on the puma560 robot
:param g_canvas: The canvas to display the robot in
:type g_canvas: class:`graphics.graphics_canvas.GraphicsCanvas`
:return: Puma560 robot
:rtype: class:`graphics.graphics_robot.GraphicalRobot`
"""
puma560 = GraphicalRobot(g_canvas, 'Puma560')
puma560.append_made_link(create_link_0(g_canvas.scene))
puma560.append_made_link(create_link_1(g_canvas.scene))
puma560.append_made_link(create_link_2(g_canvas.scene))
puma560.append_made_link(create_link_3(g_canvas.scene))
puma560.append_made_link(create_link_4(g_canvas.scene))
puma560.append_made_link(create_link_5(g_canvas.scene))
puma560.append_made_link(create_link_6(g_canvas.scene))
# Get the poses for a zero-position
puma = Puma560()
poses = puma.fkine(puma.config('qz'), alltout=True)
puma560.set_joint_poses([
SE3(), # 0 (Base doesn't change)
poses[0], # 1
poses[1], # 2
poses[2], # 3
poses[3], # 4
poses[4], # 5
poses[5] # 6
])
return puma560
def set_joint_poses(self, all_poses):
"""
Set the joint poses.
:param all_poses: List of all the new poses to set
:type all_poses: class:`spatialmath.pose3d.SE3` list
:raises UserWarning: Robot must not have 0 joints, and given poses length must equal number of joints.
"""
# Sanity checks
if self.num_joints == 0:
raise UserWarning(
"Robot has 0 joints. Create some using append_link()")
# If given a base pose, when there isn't one
if self.num_joints == len(all_poses) - 1 and all_poses[0] == SE3():
# Update the joints
for idx in range(self.num_joints):
self.joints[idx].update_pose(all_poses[idx + 1])
return
# If incorrect number of joints
if self.num_joints != len(all_poses):
err = "Number of given poses {0} does not equal number of joints {1}"
raise UserWarning(err.format(len(all_poses), self.num_joints))
# Update the joints
for idx in range(self.num_joints):
self.joints[idx].update_pose(all_poses[idx])
def A(self, q=None, fast=False):
"""
Link transform matrix
:param q: Joint coordinate (radians or metres). Not required for links
with no variable
:type q: float
:param fast: return NumPy array instead of ``SE3``
:type param: bool
:return T: link frame transformation matrix
:rtype T: SE3 or ndarray(4,4)
``LINK.A(q)`` is an SE(3) matrix that describes the rigid-body
transformation from the previous to the current link frame to
the next, which depends on the joint coordinate ``q``.
"""
if self.isjoint:
# a variable joint
if q is None:
raise ValueError("q is required for variable joints")
T = self.Ts.A @ self.v.T(q)
else:
# a fixed joint
T = self.Ts.A
if fast:
return T
else:
return SE3(T, check=False)
def test_puma560_angle_change():
"""
This test loads in the Puma560 model and changes its angles over time.
Joint angles are printed for validation.
"""
puma560 = gph.import_puma_560(g_canvas.scene)
print("Prior Poses")
puma560.print_joint_poses()
# Get the poses for a ready-position
puma = Puma560()
poses = puma.fkine(puma.config('qr'), alltout=True)
sleep(2)
puma560.set_joint_poses([
SE3(), # 0 (Base doesn't change)
poses[0], # 1
poses[1], # 2
poses[2], # 3
poses[3], # 4
poses[4], # 5
poses[5] # 6
])
print("Final Poses")
puma560.print_joint_poses()
def test_vpython_to_se3(self):
# Create a scene
scene = canvas.GraphicsCanvas3D(title="TEST VPYTHON TO SE3")
# Create a basic entity
# pos = 1, 2, 3
# X = 0, 0, -1
# Y = -1, 0, 0
# Z = 0, 1, 0
entity = box(
pos=vector(1, 2, 3),
axis=vector(0, 0, -1),
up=vector(-1, 0, 0)
)
scene.scene.waitfor("draw_complete")
# Check resulting SE3
arr = array([
[0, -1, 0, 1],
[0, 0, 1, 2],
[-1, 0, 0, 3],
[0, 0, 0, 1]
])
expected = SE3(arr)
self.assertEqual(common.vpython_to_se3(entity), expected)
def to_dict(self):
'''
to_dict() returns the shapes information in dictionary form
:returns: All information about the shape
:rtype: dict
'''
if self.stype == 'cylinder':
fk = self.wT * SE3.Rx(np.pi/2)
else:
fk = self.wT
shape = {
'stype': self.stype,
'scale': self.scale.tolist(),
'filename': self.filename,
'radius': self.radius,
'length': self.length,
't': fk.t.tolist(),
'q': r2q(fk.R).tolist(),
'v': self.v.tolist(),
'color': list(self.color)
}
return shape
def test_robot_detach_link(self):
# Update scene
self.scene.scene.title = "Test Robot Detach Link"
self.scene.grid_visibility(False)
# Create robot
robot1 = robot.GraphicalRobot(self.scene, "Robot 1")
# Add two links
robot1.append_link("r", self.se3, self.structure)
robot1.append_link("r", self.se3 * SE3().Tx(1), self.structure)
# Count num objects
num_obj = len(self.scene.scene.objects)
# Count num joints
num_joints = robot1.num_joints
# Detach
robot1.detach_link()
# Verify new object count
self.assertEqual(len(self.scene.scene.objects), num_obj - 2) # 2 = one for joint, 1 for ref frame
# Verify new joint count
self.assertEqual(robot1.num_joints, num_joints - 1) # Taken away 1 joint
# Create new empty robot
robot2 = robot.GraphicalRobot(self.scene, "Robot 2")
# Attempt to detach from empty
self.assertRaises(UserWarning, robot2.detach_link)
