def getCartImpWristTraj(self, js, goal_T_B_W): init_js = copy.deepcopy(js) init_T_B_W = self.fk_solver.calculateFk("right_arm_7_link", init_js) T_B_Wd = goal_T_B_W T_B_W_diff = PyKDL.diff(init_T_B_W, T_B_Wd, 1.0) steps = int(max(T_B_W_diff.vel.Norm() / 0.05, T_B_W_diff.rot.Norm() / (20.0 / 180.0 * math.pi))) if steps < 2: steps = 2 self.updateJointLimits(init_js) self.updateMimicJoints(init_js) q_list = [] for f in np.linspace(0.0, 1.0, steps): T_B_Wi = PyKDL.addDelta(init_T_B_W, T_B_W_diff, f) q_out = self.fk_solver.simulateTrajectory("right_arm_7_link", init_js, T_B_Wi) if q_out == None: # print "error: getCartImpWristTraj: ", str(f) return None q_list.append(q_out) for i in range(7): joint_name = self.fk_solver.ik_joint_state_name["right_arm_7_link"][i] init_js[joint_name] = q_out[i] return q_list
def handle_cart_cmd(self, msg): side = self.side try: # Get the current position of the hydra hydra_frame = fromTf(self.listener.lookupTransform( '/hydra_base', '/hydra_'+self.SIDE_STR[side]+'_grab', rospy.Time(0))) # Get the current position of the end-effector tip_frame = fromTf(self.listener.lookupTransform( '/world', self.tip_link, rospy.Time(0))) # Capture the current position if we're starting to move if not self.deadman_engaged: self.deadman_engaged = True self.cmd_origin = hydra_frame self.tip_origin = tip_frame else: self.deadman_max = max(self.deadman_max, msg.axes[self.DEADMAN[side]]) # Update commanded TF frame cmd_twist = kdl.diff(self.cmd_origin, hydra_frame) cmd_twist.vel = self.scale*self.deadman_max*cmd_twist.vel self.cmd_frame = kdl.addDelta(self.tip_origin, cmd_twist) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as ex: rospy.logwarn(str(ex))
def handle_cart_cmd(self, scaling): """""" try: # Get the current position of the hydra input_frame = fromTf(self.listener.lookupTransform( self.input_ref_frame_id, self.input_frame_id, rospy.Time(0))) # Get the current position of the end-effector tip_frame = fromTf(self.listener.lookupTransform( self.input_ref_frame_id, self.tip_link, rospy.Time(0))) # Capture the current position if we're starting to move if not self.deadman_engaged: self.deadman_engaged = True self.cmd_origin = input_frame self.tip_origin = tip_frame else: self.cart_scaling = scaling # Update commanded TF frame cmd_twist = kdl.diff(self.cmd_origin, input_frame) cmd_twist.vel = self.scale*self.cart_scaling*cmd_twist.vel #rospy.logwarn(cmd_twist) self.cmd_frame = kdl.addDelta(self.tip_origin, cmd_twist) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as ex: rospy.logwarn(str(ex))
def integ_error(self, twist_err, dT=0.01): """Apply timestep-wise error integration. """ T = PyKDL.addDelta(self.prev_frame, twist_err, dT) # --> calculate the Integral #T = PyKDL.addDelta(self.init_frame, twist_err, self.traj_elapse) #return PyKDL.diff(self.init_frame, self.cur_frame, self.traj_elapse) return PyKDL.diff(self.init_frame, T, self.traj_elapse)
def calculateWrenchesForTransportTask(self, ext_wrenches_W, transport_T_B_O): ext_wrenches_O = [] for i in range(len(transport_T_B_O)-1): diff_B_O = PyKDL.diff(transport_T_B_O[i], transport_T_B_O[i+1]) # simulate object motion and calculate expected wrenches for t in np.linspace(0.0, 1.0, 5): T_B_Osim = PyKDL.addDelta(transport_T_B_O[i], diff_B_O, t) T_Osim_B = T_B_Osim.Inverse() for ewr in ext_wrenches_W: ext_wrenches_O.append(PyKDL.Frame(T_Osim_B.M) * ewr) return ext_wrenches_O
def test_compute_twist(self): for _ in range(100): T0 = br.transform.random() vel = np.random.rand(3) rot = np.random.rand(3) kdl_twist = PyKDL.Twist(PyKDL.Vector(*vel), PyKDL.Vector(*rot)) F0 = posemath.fromMatrix(T0) F1 = PyKDL.addDelta(F0, kdl_twist) T1 = posemath.toMatrix(F1) twist = ru.transforms.compute_twist(F0, F1) np.testing.assert_allclose(twist, np.hstack((vel, rot)))
def main(): print "\n\n==== To and from tf" tf = ((1,2,3), (1,0,0,0)) print tf print toTf(fromTf(tf)) print "\n\n==== To and from msg" p = Pose() p.orientation.x = 1 p.position.x = 4 print p print toMsg(fromMsg(p)) print "\n\n==== To and from matrix" m = numpy.array([[0,1,0,2], [0,0,1,3],[1,0,0,4],[0,0,0,1]]) print m print toMatrix(fromMatrix(m)) print "\n\n==== Math" print toTf( fromMsg(p) * fromMatrix(m)) print PyKDL.diff(fromMsg(p), fromMatrix(m)) print PyKDL.addDelta(fromMsg(p), PyKDL.diff(fromMatrix(m), fromMsg(p)), 2.0)
def move(self, desiredPose, maxForce): currentPose = self.robot.get_desired_position() currentPose.p = currentPose.p forceArray = np.empty((0, 4), float) displacements = np.array([0], float) # Remove z rotation angle = np.arccos( PyKDL.dot(desiredPose.M.UnitX(), currentPose.M.UnitX())) rot = PyKDL.Rotation.Rot(desiredPose.M.UnitZ(), angle) # Added offset representing tooltip desiredPosition = desiredPose.p - desiredPose.M.UnitZ( ) * self.toolOffset desiredPoseWithOffset = PyKDL.Frame(desiredPose.M, desiredPosition) measuredPose_previous = self.robot.get_current_position() startForce = self.force[1] while not rospy.is_shutdown(): # get current and desired robot pose (desired is the top of queue) # compute the desired twist "x_dot" from motion command xDotMotion = resolvedRates( self.resolvedRatesConfig, currentPose, desiredPoseWithOffset) # xDotMotion is type [PyKDL.Twist] currentPose = PyKDL.addDelta(currentPose, xDotMotion, self.resolvedRatesConfig['dt']) #Save displacement and force in different arrays if type(self.force) == type(None): rospy.logwarn( "Probe Service: No forces detected. Not moving and returning None" ) return None, None data = np.array([self.force]) if xDotMotion.vel.Norm() <= 0.001 and xDotMotion.rot.Norm() <= 0.1: break if self.force[1] - startForce > maxForce: rospy.logwarn("Max Force Exceeded: " + str(self.force)) break self.robot.move(currentPose, interpolate=False) self.rate.sleep() measuredPose_current = self.robot.get_current_position() currentDisplacement = measuredPose_current.p - measuredPose_previous.p # currentDisplacement = xDotMotion.vel.Norm() * self.resolvedRatesConfig['dt'] currentDisplacement = PyKDL.dot(currentDisplacement, desiredPose.M.UnitZ()) # currentDisplacement = displacements[len(displacements)-1] + currentDisplacement forceArray = np.append(forceArray, data, axis=0) displacements = np.append(displacements, [currentDisplacement]) return displacements.tolist(), forceArray.tolist()
def replan_movement(self, goal_kdl_frame, current_kdl_frame, interp_time=2): #Check if really new goal: if not self.check_if_new_goal(goal_kdl_frame): rospy.logwarn("Not a new goal") return #If we got here, the message is really different self.new_goal(goal_kdl_frame) self.set_current_pose(current_kdl_frame) #find distance rot_dist = kdl.diff(self.current_pose.M, self.dst_pose.M) if rot_dist[0] != rot_dist[0]: #getting nans here, work around bug by rotating current pose a little self.current_pose.M.DoRotZ(2 * 3.141509 / 180.0) dist = kdl.diff(kdl.Frame(self.current_pose), kdl.Frame(self.dst_pose)) #print("current pose:") #print self.current_pose.M.GetRPY() dt = 0.05 #looks smoother at 20Hz total_time = interp_time #sec times = int(total_time / dt) dx = 1.0 / times self.start_time = rospy.Time.now() self.pose_vector = [] for i in range(times + 1): if i == 0: inter_pose = self.current_pose else: inter_pose = kdl.addDelta(kdl.Frame(self.current_pose), dist, dx * i) inter_time = self.start_time + rospy.Duration(dt) * i #rospy.logwarn("Generated pose:") #print inter_pose #print inter_pose.p #print inter_pose.M.GetRPY() #rospy.logwarn("Original pose:") #print self.current_pose self.pose_vector.append([inter_time, kdl.Frame(inter_pose)])
def derive_features(frame, feature_function, dd=0.01): ''' numerically derive the task angle function around the given frame. Also computes the inverse: this yields the instantaneous rotation axes for each angle. ''' q0 = numpy.mat(feature_function(frame)) size = q0.size print q0.size jac_i = numpy.mat(numpy.zeros((q0.size, 6))) for i in range(6): twist = kdl.Twist() twist[i] = 1.0 frame_moved = kdl.addDelta(frame, twist, dd) q_i = numpy.mat( feature_function(frame_moved) ) jac_i[0:size,i] = ((q_i - q0) / dd).T # the columns of jac are six twists to be displayed jac = numpy.linalg.pinv(jac_i) return (jac_i, jac)
def move(self, desiredPose, maxForce): currentPose = self.robot.get_desired_position() currentPose.p = currentPose.p displacements = np.array([0], float) # Remove z rotation angle = np.arccos( PyKDL.dot(desiredPose.M.UnitX(), currentPose.M.UnitX())) rot = PyKDL.Rotation.Rot(desiredPose.M.UnitZ(), angle) # Added offset representing tooltip desiredPosition = desiredPose.p - desiredPose.M.UnitZ( ) * self.toolOffset desiredPoseWithOffset = PyKDL.Frame(desiredPose.M, desiredPosition) measuredPose_previous = self.robot.get_current_position() startForce = self.force[1] while not rospy.is_shutdown(): # get current and desired robot pose (desired is the top of queue) # compute the desired twist "x_dot" from motion command xDotMotion = resolvedRates( self.resolvedRatesConfig, currentPose, desiredPoseWithOffset) # xDotMotion is type [PyKDL.Twist] currentPose = PyKDL.addDelta(currentPose, xDotMotion, self.resolvedRatesConfig['dt']) if xDotMotion.vel.Norm() <= 0.001 and xDotMotion.rot.Norm() <= 0.1: break self.robot.move(currentPose, interpolate=False) self.rate.sleep() measuredPose_current = self.robot.get_current_position() currentDisplacement = measuredPose_current.p - measuredPose_previous.p currentDisplacement = PyKDL.dot(currentDisplacement, desiredPose.M.UnitZ()) displacements = np.append(displacements, [currentDisplacement]) return displacements.tolist()
def compute_increment(goal_features, feature_set, frame, dd=0.1): tw = compute_direction(goal_features, feature_set, frame) print 'twist: '+ str(tw) return kdl.addDelta(frame, tw, dd)
def pose_oplus(kdl_pose, step): t = PyKDL.Twist() for i in range(pose_width): t[i] = step[i,0] next_pose = kdl_pose * PyKDL.addDelta(PyKDL.Frame(), t, 1.0) return next_pose
def find_checkerboard_service(self, req): poses = [] min_x = self.ros_image.width min_y = self.ros_image.height max_x = 0 max_y = 0 for i in range(10): rospy.sleep(0.5) #copy the image over with self.mutex: image = self.ros_image result = self.find_checkerboard_pose(image, req.corners_x, req.corners_y, req.spacing_x, req.spacing_y, self.width_scaling, self.height_scaling) if result is None: rospy.logerr("Couldn't find a checkerboard") continue p, corners = result poses.append(p) for j in range(corners.cols): x = cv.GetReal2D(corners, 0, j) y = cv.GetReal2D(corners, 1, j) min_x = min(min_x, x) max_x = max(max_x, x) min_y = min(min_y, y) max_y = max(max_y, y) # convert all poses to twists twists = [] for p in poses: twists.append( PyKDL.diff(PyKDL.Frame.Identity(), posemath.fromMsg(p.pose))) # get average twist twist_res = PyKDL.Twist() PyKDL.SetToZero(twist_res) for t in twists: for i in range(3): twist_res.vel[i] += t.vel[i] / len(poses) twist_res.rot[i] += t.rot[i] / len(poses) # get noise noise_rot = 0 noise_vel = 0 for t in twists: n_vel = (t.vel - twist_res.vel).Norm() n_rot = (t.rot - twist_res.rot).Norm() if n_vel > noise_vel: noise_vel = n_vel if n_rot > noise_rot: noise_rot = n_rot # set service resul pose_res = PyKDL.addDelta(PyKDL.Frame.Identity(), twist_res, 1.0) pose_msg = PoseStamped() pose_msg.header = poses[0].header pose_msg.pose = posemath.toMsg(pose_res) return GetCheckerboardPoseResponse(pose_msg, min_x, max_x, min_y, max_y, noise_vel, noise_rot)
def my_adddelta(frame, twist, dt): return ( kdlframe_to_narray( PyKDL.addDelta( narray_to_kdlframe(frame), narray_to_kdltwist(twist), dt)))
def find_checkerboard_service(self, req): poses = [] min_x = self.ros_image.width min_y = self.ros_image.height max_x = 0 max_y = 0 for i in range(10): rospy.sleep(0.5) #copy the image over with self.mutex: image = self.ros_image result = self.find_checkerboard_pose(image, req.corners_x, req.corners_y, req.spacing_x, req.spacing_y, self.width_scaling, self.height_scaling) if result is None: rospy.logerr("Couldn't find a checkerboard") continue p, corners = result poses.append(p) for j in range(corners.cols): x = cv.GetReal2D(corners, 0, j) y = cv.GetReal2D(corners, 1, j) min_x = min(min_x, x) max_x = max(max_x, x) min_y = min(min_y, y) max_y = max(max_y, y) # convert all poses to twists twists = [] for p in poses: twists.append(PyKDL.diff(PyKDL.Frame.Identity(), posemath.fromMsg(p.pose))) # get average twist twist_res = PyKDL.Twist() PyKDL.SetToZero(twist_res) for t in twists: for i in range(3): twist_res.vel[i] += t.vel[i]/len(poses) twist_res.rot[i] += t.rot[i]/len(poses) # get noise noise_rot = 0 noise_vel = 0 for t in twists: n_vel = (t.vel - twist_res.vel).Norm() n_rot = (t.rot - twist_res.rot).Norm() if n_vel > noise_vel: noise_vel = n_vel if n_rot > noise_rot: noise_rot = n_rot # set service resul pose_res = PyKDL.addDelta(PyKDL.Frame.Identity(), twist_res, 1.0) pose_msg = PoseStamped() pose_msg.header = poses[0].header pose_msg.pose = posemath.toMsg(pose_res) return GetCheckerboardPoseResponse(pose_msg, min_x, max_x, min_y, max_y, noise_vel, noise_rot)
def integ_error(self, twist_err, dT): """Apply timestep-wise error integration. """ self.integ_frame = PyKDL.addDelta(self.integ_frame, twist_err, dT) return PyKDL.diff(self.init_frame, self.integ_frame, self.traj_elapse)
def pose_oplus(kdl_pose, step): t = PyKDL.Twist() for i in range(pose_width): t[i] = step[i, 0] next_pose = kdl_pose * PyKDL.addDelta(PyKDL.Frame(), t, 1.0) return next_pose
def run(self): commandedPose = self.robot.get_desired_position() # find start pose desiredPose = commandedPose # match original desired position at start tic = time() pauseTime = 30 while not rospy.is_shutdown(): toc = (time() - tic) if np.floor(toc / pauseTime) == 1: # ipdb.set_trace() tic = time() # Publish trajectory length at all times self.trajStatusPub.publish(self.trajIndex) # Check if there are any trajectories try: desiredPose = self.trajectory[0] except IndexError: # If no trajectory don't move, exit continue # get current and desired robot pose (desired is the top of queue) currentPose = self.robot.get_current_position( ) # this is used to capture the error # Update the wrist orientation so that the desired orientation matches the environment if self.resolvedRatesConfig['b_wristOrient'] and self.inContact: zcur = PyKDL.Vector(currentPose.M[0, 2], currentPose.M[1, 2], currentPose.M[2, 2]) zdes = -PyKDL.Vector(self.getAverageForce()) zdes = self.coneLimit(zdes, self.resolvedRatesConfig['zref'], np.pi / 2) zdes.Normalize() normalDirection = self.movingDirection * PyKDL.Vector( self.forceProfile['defaultDir'][0], self.forceProfile['defaultDir'][1], self.forceProfile['defaultDir'][2]) zdes = self.projectNullSpace2(normalDirection, zdes) axis = zcur * zdes crossNorm = axis.Normalize() dotResult = PyKDL.dot(zcur, zdes) angle = math.atan2(crossNorm, dotResult) diffTwist = PyKDL.Twist(PyKDL.Vector(), axis * angle) desiredPose = PyKDL.addDelta(desiredPose, diffTwist, 1) pass # compute the desired twist "x_dot" from motion command [PyKDL.Twist] [xDotMotion, goalPoseReached] = self.resolvedRates(currentPose, \ desiredPose) # Do hybrid force motion if using the force controller if self.forceProfile['b_forceOn']: # compute the desired twist "x_dot" from force command [PyKDL.Twist] #TRY NOT DOING THIS! if self.resolvedRatesConfig['b_wristOrient'] and self.inContact: try: forceCtrlDir = zdes except NameError: forceCntrlDir = PyKDL.Vector(\ self.forceProfile['defaultDir'][0], self.forceProfile['defaultDir'][1], self.forceProfile['defaultDir'][2]) else: forceCtrlDir = self.updateForceControlDir() xDotForce = self.forceAdmittanceControl(forceCtrlDir) [xDot, goalPoseReached ] = self.hybridPosForce(xDotMotion, xDotForce, forceCtrlDir) else: xDot = xDotMotion # Check whether we have reached our goal if goalPoseReached: # ipdb.set_trace() # When first reached the first goal pose, turn on the force controller self.forceProfile['b_forceOn'] = True if len(self.trajectory) > 1 and self.inContact: # self.movingDirection=PyKDL.Vector((PyKDL.diff(self.trajectory[1],self.trajectory[0])).vel) # self.movingDirection.Normalize() self.trajectory.popleft() self.trajIndex = self.trajIndex + 1 print("Trajectories finished: " + str(self.trajIndex)) pass # else: # ipdb.set_trace() # apply the desired twist on the currnet pose dt = self.resolvedRatesConfig['dt'] commandedPose = PyKDL.addDelta(commandedPose, xDot, dt) #ipdb.set_trace() # Move the robot self.robot._arm__move_frame(commandedPose, interpolate=False) self.rate.sleep()
from geometry_msgs.msg import Twist rospy.init_node("moveable_frame") frame = kdl.Frame() twist = kdl.Twist() def callback(msg): global twist lin = kdl.Vector(msg.linear.x, msg.linear.y, msg.linear.z) rot = kdl.Vector(msg.angular.x, msg.angular.y, msg.angular.z) twist = kdl.Twist(lin, rot) frame_name = rospy.get_param("~frame_name", "/frame") parent_name = rospy.get_param("~parent_name", "/base_link") f = rospy.get_param("~rate", 30) rospy.Subscriber("twist", Twist, callback) tf_pub = tf.TransformBroadcaster() rate = rospy.Rate(f) while not rospy.is_shutdown(): t = kdl.Twist(twist) # frame = kdl.addDelta(frame, t, 1.0/f) # global mode frame = kdl.addDelta(frame, kdl.Frame(frame.M) * t, 1.0 / f) # local mode tf_pub.sendTransform(frame.p, frame.M.GetQuaternion(), rospy.Time.now(), frame_name, parent_name) rate.sleep()
def execute_cb(self, goal): # helper variables success = True if self.robot_state.joint_impedance_active: self._result.error_code = 0 self._as.set_succeeded(self._result) print "MoveCartesianTrajectory ", self._action_name, " points: ", len(goal.trajectory.points) init_T_B_W = self.robot_state.fk_solver.calculateFk(self.wrist_name, self.robot_state.getJsPos()) init_T_B_T = init_T_B_W * self.robot_state.T_W_T[self.wrist_name] current_dest_point_idx = 0 while True: if self._as.is_preempt_requested(): rospy.loginfo("%s: Preempted" % self._action_name) self._as.set_preempted() return time_now = rospy.Time.now() time_from_start = (time_now - goal.trajectory.header.stamp).to_sec() if time_from_start <= 0: rospy.sleep(0.01) continue while time_from_start > goal.trajectory.points[current_dest_point_idx].time_from_start.to_sec(): current_dest_point_idx += 1 if current_dest_point_idx >= len(goal.trajectory.points): break if current_dest_point_idx >= len(goal.trajectory.points): break dest_time = goal.trajectory.points[current_dest_point_idx].time_from_start.to_sec() dest_T_B_T = pm.fromMsg(goal.trajectory.points[current_dest_point_idx].pose) if current_dest_point_idx > 0: prev_time = goal.trajectory.points[current_dest_point_idx - 1].time_from_start.to_sec() prev_T_B_T = pm.fromMsg(goal.trajectory.points[current_dest_point_idx - 1].pose) else: prev_time = 0.0 prev_T_B_T = init_T_B_T f = (time_from_start - prev_time) / (dest_time - prev_time) if f < 0 or f > 1: print "error: MoveCartesianTrajectory f: ", str(f) diff_T_B_T = PyKDL.diff(prev_T_B_T, dest_T_B_T, 1.0) T_B_Ti = PyKDL.addDelta(prev_T_B_T, diff_T_B_T, f) T_B_Wi = T_B_Ti * self.robot_state.T_W_T[self.wrist_name].Inverse() q_out = self.robot_state.fk_solver.simulateTrajectory(self.wrist_name, self.robot_state.getJsPos(), T_B_Wi) if q_out == None: self._result.error_code = -3 # PATH_TOLERANCE_VIOLATED self._as.set_aborted(self._result) return for i in range(7): joint_name = self.robot_state.fk_solver.ik_joint_state_name[self.wrist_name][i] self.robot_state.js.position[self.robot_state.joint_name_idx_map[joint_name]] = q_out[i] self.robot_state.arm_cmd[self.wrist_name] = T_B_Ti # publish the feedback self._feedback.header.stamp = time_now self._feedback.desired = pm.toMsg(T_B_Ti) self._feedback.actual = pm.toMsg(T_B_Ti) self._feedback.error = pm.toMsg(PyKDL.Frame()) self._as.publish_feedback(self._feedback) rospy.sleep(0.01) self._result.error_code = 0 self._as.set_succeeded(self._result)