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)
def servo_cb(self, goal):
if goal.stamped_pose.header.frame_id == '':
goal.stamped_pose.header.frame_id = self.base_frame
transformed = self.tf_listener.transformPose(self.base_frame,
goal.stamped_pose)
wTep = transforms.pose_msg_to_trans(transformed.pose)
Y = 0.005
Q = Y * np.eye(7)
arrived = False
rate = rospy.Rate(200)
while not arrived and self.state.errors == 0:
msg = JointVelocity()
v, arrived = rp.p_servo(self.configuration.T, wTep,
goal.scaling if goal.scaling else 0.6)
Aeq = self.configuration.Je
beq = v.reshape((6, ))
c = -self.configuration.Jm.reshape((7, ))
dq = qp.solve_qp(Q, c, None, None, Aeq, beq)
self.joint_velocity_cb(JointVelocity(joints=dq.tolist()))
rate.sleep()
return self.pose_servo_server.set_succeeded(
ServoToPoseResult(result=0 if self.state.errors == 0 else 1))
def step_r(self, robot, Ts):
e, m, _ = self.state(robot, Ts)
v, robot.arrived = rp.p_servo(robot.fkine(), Ts, 1, threshold=0.17)
if np.linalg.matrix_rank(robot.jacobe()) < 6:
robot.s = True
robot.fail = True
robot.qd = np.linalg.inv(robot.jacobe()) @ v
robot.m += m
robot.it += 1
if self._check_limit(robot):
robot.fail = True
def step_q(self, robot, Ts):
ps = 0.05
pi = 0.9
e, m, _ = self.state(robot, Ts)
v, robot.arrived = rp.p_servo(robot.fkine(), Ts, 1, threshold=0.17)
Y = 0.01
Ain = np.zeros((12, 12))
bin = np.zeros(6 + 6)
for i in range(robot.n):
if robot.q[i] - self.qlim[0, i] <= pi:
bin[i] = -1.0 * (((self.qlim[0, i] - robot.q[i]) + ps) /
(pi - ps))
Ain[i, i] = -1
if self.qlim[1, i] - robot.q[i] <= pi:
bin[i] = ((self.qlim[1, i] - robot.q[i]) - ps) / (pi - ps)
Ain[i, i] = 1
Q = np.eye(6 + 6)
Q[:6, :6] *= Y
Q[6:, 6:] = (1 / e) * np.eye(6)
Aeq = np.c_[robot.jacobe(), np.eye(6)]
beq = v.reshape((6, ))
c = np.r_[-robot.jacobm().reshape((6, )), np.zeros(6)]
qd = qp.solve_qp(Q, c, Ain, bin, Aeq, beq)
if np.any(np.isnan(qd)):
robot.fail = True
robot.s = True
robot.qd = robot.qz
else:
robot.qd = qd[:6]
robot.m += m
robot.it += 1
if self._check_limit(robot):
robot.fail = True
import spatialmath as sm
import numpy as np
import time
env = rp.backend.PyPlot()
env.launch('Panda Resolved-Rate Motion Control Example')
panda = rp.PandaMDH()
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()
env.step(dt)
Tr, Tq = find_pose()
start = time.time()
while (((not arrivedq and not failq) or (not arrivedr and not failr))
and (time.time() - start) < tmax):
if not arrivedr and not failr:
try:
mq += urQuad.manipulability()
if np.linalg.matrix_rank(urRrmc.jacobe()) < 6:
s[i, 2] = True
failr = True
vr, arrivedr = rp.p_servo(urRrmc.fkine(), Tr, 1, threshold=0.1)
urRrmc.qd = np.linalg.inv(urRrmc.jacobe()) @ vr
it1 += 1
except np.linalg.LinAlgError:
failr = True
s[i, 2] = True
else:
urRrmc.qd = np.zeros(n)
if not arrivedq and not failq:
try:
mr += urRrmc.manipulability()
vq, arrivedq = rp.p_servo(urQuad.fkine(), Tq, 1, threshold=0.1)
eTep = urQuad.fkine().inv() * Tq
e = np.sum(np.abs(np.r_[eTep.t, eTep.rpy() * np.pi / 180]))
def go(self):
# startQuat = quatMult(array([1.0, 0.0, 0.0, 0.0]), euler2quat([np.pi/4,0,0]))
# print(startQuat)
print("============ Press Enter to move to initial pose...")
raw_input()
# straight down, z=0.155 hits table
# start_pose = [0.35, 0.0, 0.18, 0.89254919, -0.36948312, 0.23914479, -0.09822433] # for pushing
# start_pose = [0.60, 0.0, 0.155, 0.92387953, -0.38268343, 0., 0.] # straight down
# start_pose = [0.3375, 0.2625, 0.16, 0.92387953, -0.38268343, 0., 0.] # straight down
# self.pc.goto_pose(start_pose, velocity=0.1)
# self.pc.set_gripper(0.0)
start_joint_angles = [-0.011, 0.261, 0.014, -2.941, 0.010, 3.725, 0.776]
self.pc.goto_joints(start_joint_angles)
self.pc.set_gripper(0.025)
rospy.sleep(1.0)
# loop actions
while not rospy.is_shutdown():
print("============ Press Enter to switch to velocity control...")
raw_input()
# Initialize target pose
self.wTep = array(self.robot_state.O_T_EE).reshape(4,4,order='F')
# START
self.cs.switch_controller('velocity')
r = rospy.Rate(self.curr_velocity_publish_rate)
arrived = False
ctr = 0
start_time = 0
while 1:
# Get current joint angles
self.panda.q = np.array(self.robot_state.q)
# Update current ee pose
wTe = array(self.robot_state.O_T_EE).reshape(4,4,order='F')
# Desired end-effecor spatial velocity
v, arrived = rp.p_servo(wTe, self.wTep, gain=2.0, threshold=0.1)
# Solve for the joint velocities dq
dq = np.matmul(np.linalg.pinv(self.panda.Je), v)
# Stopping criteria: check ee pos, offset bt tip and ee is 7.7cm
if self.robot_state.O_T_EE[-4] > 0.67 or abs(self.robot_state.O_T_EE[-3]) > 0.25:
break
# Update wTep in 5Hz
ctr += 1
if ctr >= self.curr_velocity_publish_rate/self.update_rate:
ctr = 0
self.__trigger_update()
# print(time.time()-start_time)
# start_time = time.time()
# # Check cartesian contact
# if any(self.robot_state.cartesian_contact):
# self.stop()
# rospy.logerr('Detected cartesian contact during velocity control loop.')
# break
# Send joint velocity cmd
v = Float64MultiArray()
v.data = dq
self.curr_velo_pub.publish(v)
r.sleep()
# Send zero velocities for a few seconds
ctr = 0
while ctr < 100:
self.stop()
ctr += 1
r.sleep()
# Back
print("============ Press Enter to home...")
raw_input()
start_joint_angles = [-0.011, 0.261, 0.014, -2.941, 0.010, 3.725, 0.776]
self.cs.switch_controller('moveit')
self.pc.goto_joints(start_joint_angles)
self.pc.set_gripper(0.025)