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()