def test_p_servo(self):
a = sm.SE3()
b = sm.SE3.Rx(0.7) * sm.SE3.Tx(1)
c = sm.SE3.Tz(0.59)
v0, arrived0 = rp.p_servo(a, b)
v1, _ = rp.p_servo(a.A, b.A)
_, arrived1 = rp.p_servo(a, c, threshold=0.6)
ans = np.array([2, 0, 0, 1.4, -0, 0])
nt.assert_array_almost_equal(v0, ans, decimal=4)
nt.assert_array_almost_equal(v1, ans, decimal=4)
self.assertFalse(arrived0)
self.assertTrue(arrived1)
import spatialmath as sm
import numpy as np
import time
env = PyPlot()
env.launch('Panda Resolved-Rate Motion Control Example')
panda = rtb.models.DH.Panda()
panda.q = panda.qr
Tep = panda.fkine(panda.q) * sm.SE3.Tx(-0.2) * sm.SE3.Ty(0.2) * sm.SE3.Tz(0.2)
arrived = False
env.add(panda)
dt = 0.05
while not arrived:
start = time.time()
v, arrived = rtb.p_servo(panda.fkine(panda.q), Tep, 1)
panda.qd = np.linalg.pinv(panda.jacobe(panda.q)) @ v
env.step(dt)
stop = time.time()
if stop - start < dt:
time.sleep(dt - (stop - start))
# Uncomment to stop the plot from closing
# env.hold()
T = SE3(0.7, 0.2, 0.1) * SE3.OA([0, 1, 0], [0, 0, -1])
solution = robot.ikine_LM(T)
traj = jtraj(robot.qz, solution.q, 50)
robot.plot(traj.q, backend='swift')
# Make and instance of the Swift simulator and open it
env = Swift()
env.launch()
# Make a robot model and set its joint angles to the ready joint configuration
robot = rtb.models.Panda()
robot.q = robot.qr
# Set a desired and effector pose an an offset from the current end-effector pose
Tep = robot.fkine(robot.q) * SE3.Tx(0.2) * SE3.Ty(0.2) * SE3.Tz(0.45)
# Add the robot to the simulator
env.add(robot)
time.sleep(3)
# Simulate the robot while it has not arrived at the goal
arrived = False
while not arrived:
# Work out the required end-effector velocity to go towards the goal
v, arrived = rtb.p_servo(robot.fkine(robot.q), Tep, 1)
# Set the robot's joint velocities (calculate pseudoinverse(J) * v)
robot.qd = np.linalg.pinv(robot.jacobe(robot.q)) @ v
# Step the simulator by 15 milliseconds
env.step(0.015)
arrived = False
while not arrived:
# The pose of the Panda's end-effector
Te = panda.fkine()
# Transform from the end-effector to desired pose
eTep = Te.inv() * Tep
# Spatial error
e = np.sum(np.abs(np.r_[eTep.t, eTep.rpy() * np.pi / 180]))
# Calulate the required end-effector spatial velocity for the robot
# to approach the goal. Gain is set to 1.0
v, arrived = rtb.p_servo(Te, Tep, 1.0)
# Gain term (lambda) for control minimisation
Y = 0.01
# Quadratic component of objective function
Q = np.eye(n + 6)
# Joint velocity component of Q
Q[:n, :n] *= Y
# Slack component of Q
Q[n:, n:] = (1 / e) * np.eye(6)
# The equality contraints
Aeq = np.c_[panda.jacobe(), np.eye(6)]
arrived = False
while not arrived:
# The pose of the Panda's end-effector
Te = panda.fkine()
# Transform from the end-effector to desired pose
eTep = Te.inv() * Tep
# Spatial error
e = np.sum(np.abs(np.r_[eTep.t, eTep.rpy() * np.pi / 180]))
# Calulate the required end-effector spatial velocity for the robot
# to approach the goal. Gain is set to 1.0
v, arrived = rtb.p_servo(Te, Tep, 0.5, 0.05)
# Gain term (lambda) for control minimisation
Y = 0.01
# Quadratic component of objective function
Q = np.eye(n + 6)
# Joint velocity component of Q
Q[:n, :n] *= Y
# Slack component of Q
Q[n:, n:] = (1 / e) * np.eye(6)
# The equality contraints
Aeq = np.c_[panda.jacobe(), np.eye(6)]
def servo_cb(self, goal: ServoToPoseGoal) -> None:
"""
ROS Action Server callback:
Servos the end-effector to the cartesian pose indicated by goal
:param goal: [description]
:type goal: ServoToPoseGoal
This callback makes use of the roboticstoolbox p_servo function
to generate velocities at each timestep.
"""
if self.moving:
self.preempt()
with self.lock:
goal_pose = goal.pose_stamped
if goal_pose.header.frame_id == '':
goal_pose.header.frame_id = self.base_link.name
goal_pose = self.tf_listener.transformPose(
self.base_link.name,
goal_pose
)
pose = goal_pose.pose
target = SE3(pose.position.x, pose.position.y, pose.position.z) * UnitQuaternion([
pose.orientation.w,
pose.orientation.x,
pose.orientation.y,
pose.orientation.z
]).SE3()
arrived = False
self.moving = True
while not arrived and not self.preempted:
velocities, arrived = rtb.p_servo(
self.fkine(self.q, start=self.base_link, end=self.gripper),
target,
min(3, goal.gain) if goal.gain else 2,
threshold=goal.threshold if goal.threshold else 0.005
)
self.event.clear()
self.j_v = np.linalg.pinv(
self.jacobe(self.q)) @ velocities
self.last_update = timeit.default_timer()
self.event.wait()
self.moving = False
result = not self.preempted
self.preempted = False
# self.qd *= 0
if result:
self.pose_servo_server.set_succeeded(
ServoToPoseResult(success=True)
)
else:
self.pose_servo_server.set_aborted(
ServoToPoseResult(success=False)
)
import spatialmath as sm
import numpy as np
import time
env = rp.backends.PyPlot()
env.launch('Panda Resolved-Rate Motion Control Example')
panda = rp.models.DH.Panda()
panda.q = panda.qr
Tep = panda.fkine() * sm.SE3.Tx(-0.2) * sm.SE3.Ty(0.2) * sm.SE3.Tz(0.2)
arrived = False
env.add(panda)
dt = 0.05
while not arrived:
start = time.time()
v, arrived = rp.p_servo(panda.fkine(), Tep, 1)
panda.qd = np.linalg.pinv(panda.jacobe()) @ v
env.step(50)
stop = time.time()
if stop - start < dt:
time.sleep(dt - (stop - start))
# Uncomment to stop the plot from closing
# env.hold()
import roboticstoolbox as rtb
import spatialmath as sm
import numpy as np
import qpsolvers as qp
import time
# Launch the simulator Swift
env = rtb.backends.Swift()
env.launch()
# Create a Panda robot object
r = rtb.models.Frankie()
# Set joint angles to ready configuration
r.q = r.qr
# Add the puma to the simulator
env.add(r)
time.sleep(1)
Tep = r.fkine(r.q) * sm.SE3.Tx(1.0) * sm.SE3.Ty(1.0)
arrived = False
dt = 0.05
while not arrived:
v, arrived = rtb.p_servo(r.fkine(r.q), Tep, 0.1)
r.qd = np.linalg.pinv(r.jacobe(r.q)) @ v
env.step(dt)
def step():
# The pose of the Panda's end-effector
Te = panda.fkine(panda.q)
# Transform from the end-effector to desired pose
eTep = Te.inv() * Tep
# Spatial error
e = np.sum(np.abs(np.r_[eTep.t, eTep.rpy() * np.pi / 180]))
# Calulate the required end-effector spatial velocity for the robot
# to approach the goal. Gain is set to 1.0
v, arrived = rtb.p_servo(Te, Tep, 0.5, 0.05)
# Gain term (lambda) for control minimisation
Y = 0.01
# Quadratic component of objective function
Q = np.eye(n + 6)
# Joint velocity component of Q
Q[:n, :n] *= Y
# Slack component of Q
Q[n:, n:] = (1 / e) * np.eye(6)
# The equality contraints
Aeq = np.c_[panda.jacobe(panda.q), np.eye(6)]
beq = v.reshape((6, ))
# The inequality constraints for joint limit avoidance
Ain = np.zeros((n + 6, n + 6))
bin = np.zeros(n + 6)
# The minimum angle (in radians) in which the joint is allowed to approach
# to its limit
ps = 0.05
# The influence angle (in radians) in which the velocity damper
# becomes active
pi = 0.9
# Form the joint limit velocity damper
Ain[:n, :n], bin[:n] = panda.joint_velocity_damper(ps, pi, n)
# For each collision in the scene
for collision in collisions:
# Form the velocity damper inequality contraint for each collision
# object on the robot to the collision in the scene
c_Ain, c_bin = panda.link_collision_damper(
collision,
panda.q[:n],
0.3,
0.05,
1.0,
start=panda.link_dict['panda_link1'],
end=panda.link_dict['panda_hand'])
# If there are any parts of the robot within the influence distance
# to the collision in the scene
if c_Ain is not None and c_bin is not None:
c_Ain = np.c_[c_Ain, np.zeros((c_Ain.shape[0], 6))]
# Stack the inequality constraints
Ain = np.r_[Ain, c_Ain]
bin = np.r_[bin, c_bin]
# Linear component of objective function: the manipulability Jacobian
c = np.r_[-panda.jacobm(panda.q).reshape((n, )), np.zeros(6)]
# The lower and upper bounds on the joint velocity and slack variable
lb = -np.r_[panda.qdlim[:n], 10 * np.ones(6)]
ub = np.r_[panda.qdlim[:n], 10 * np.ones(6)]
# Solve for the joint velocities dq
qd = qp.solve_qp(Q, c, Ain, bin, Aeq, beq, lb=lb, ub=ub)
# Apply the joint velocities to the Panda
panda.qd[:n] = qd[:n]
# Step the simulator by 50 ms
env.step(0.01)
return arrived
panda.q = panda.qr
vell = panda.vellipse(centre='ee')
env = rp.backends.PyPlot()
env.launch(
'Panda Velocity Ellipse Example',
limits=[-0.2, 0.6, -0.4, 0.4, 0, 0.8])
Tep = panda.fkine() * sm.SE3.Tx(-0.3) * sm.SE3.Tz(0.3) * sm.SE3.Rz(np.pi)
arrived = False
env.add(panda)
env.add(vell)
dt = 0.05
while not arrived:
start = time.time()
v, arrived = rp.p_servo(panda.fkine(panda.q), Tep, 0.5)
panda.qd = np.linalg.pinv(panda.jacobe(panda.q)) @ v
env.step(50)
stop = time.time()
if stop - start < dt:
time.sleep(dt - (stop - start))
env.hold()
