def leaveHandle(): print "Wyjazd z klamki..." Goal = PyKDL.Vector( doorR_x + 0.1 * math.cos(alfa) + 0.25 * math.sin(alfa), doorR_y + 0.1 * math.sin(alfa) - 0.25 * math.cos(alfa), doorR_z + 0.1) T_B_Door = PyKDL.Frame(PyKDL.Rotation.RotZ(0.25 * math.pi + alfa), Goal) if not velma.moveCartImpRight( [T_B_Door], [10.0], None, None, None, None, PyKDL.Wrench(PyKDL.Vector(100, 100, 0.7), PyKDL.Vector(0.7, 0.7, 0.7)), start_time=0.5, path_tol=PyKDL.Twist(PyKDL.Vector(0.2, 0.2, 1), PyKDL.Vector(1, 1, 1))): exitError(8) if velma.waitForEffectorRight() != 0: print "Velma: :p" rospy.sleep(0.5) resetPoseTol() rospy.sleep(0.5)
def twist_cb(self, msg): twist = PyKDL.Twist() state_button_1 = msg.paddles[1].buttons[0] state_button_4 = msg.paddles[1].buttons[4] state_button_5 = msg.paddles[1].buttons[5] if state_button_1: para_vel = -1 else: para_vel = 1 if state_button_5: para_ang = 1 else: para_ang = 0 if state_button_4: para_trans = 1 else: para_trans = 0 twist[0] = msg.paddles[1].joy[0] twist[1] = msg.paddles[1].joy[1] twist[2] = para_vel*msg.paddles[1].trigger twist[3] = 0#para_ang*msg.paddles[1].joy[0]#msg.angular.x twist[4] = 0#para_ang*msg.paddles[1].joy[1]#msg.angular.y twist[5] = 0#para_ang*para_vel*msg.paddles[1].trigger#msg.angular.z self.twist = self._T_baseoffset_inverse * twist pass
def moveToDoor(): print "Ruch do dzwiczek..." Goal = PyKDL.Vector( 1 / 3 * doorL_x + 2 / 3 * doorR_x + 0.5 * math.cos(alfa), 1 / 3 * doorL_y + 2 / 3 * doorR_y + 0.5 * math.sin(alfa), doorR_z + 0.1) T_B_Door = PyKDL.Frame(PyKDL.Rotation.RotZ(alfa), Goal) if not velma.moveCartImpRight( [T_B_Door], [10.0], None, None, None, None, PyKDL.Wrench(PyKDL.Vector(100, 100, 0.7), PyKDL.Vector(0.7, 0.7, 0.7)), start_time=0.5, path_tol=PyKDL.Twist(PyKDL.Vector(0.2, 0.2, 1), PyKDL.Vector(1, 1, 1))): exitError(8) if velma.waitForEffectorRight() != 0: print "Velma: Dotykam drzwi :p" rospy.sleep(0.5) resetPoseTol() rospy.sleep(0.5)
def moveToHandle(): print "Ruch do klamki..." DoorR = velma.getTf( "B", "right_door" ) #getTf(frame_from, frame_to); "B"-robot base frame (torso_base); returns: PyKDL.Frame transformation from base frame to target frame doorR_x, doorR_y, doorR_z = DoorR.p DoorL = velma.getTf( "B", "left_door" ) #getTf(frame_from, frame_to); "B"-robot base frame (torso_base); returns: PyKDL.Frame transformation from base frame to target frame doorL_x, doorL_y, doorL_z = DoorL.p position = velma.getLastJointState() Goal = PyKDL.Vector(doorL_x, doorL_y, doorR_z + 0.1) T_B_Door = PyKDL.Frame(PyKDL.Rotation.RotZ(0), Goal) if not velma.moveCartImpRight( [T_B_Door], [10.0], None, None, None, None, PyKDL.Wrench(PyKDL.Vector(100, 100, 0.7), PyKDL.Vector(0.7, 0.7, 0.7)), start_time=0.5, path_tol=PyKDL.Twist(PyKDL.Vector(0.2, 0.2, 1), PyKDL.Vector(1, 1, 1))): exitError(8) if velma.waitForEffectorRight() != 0: print "Velma: Dotykam klamki :p" rospy.sleep(0.5) resetPoseTol() rospy.sleep(0.5)
def forceAdmittanceControl(self, forceCtrlDir): # compute the desired force magnitude curTime = (2 * np.pi) / self.forceProfile['period'] * time() sinamplitudeMag = np.sin(curTime) * abs( self.forceProfile['amplitude']) / 2 + abs( self.forceProfile['amplitude']) / 2 cosVal = np.cos(curTime) if cosVal <= 0 and self.lastCosVal >= 0: self.maxSinePublisher.publish(Bool(True)) self.lastCosVal = cosVal fRefMag = sinamplitudeMag + abs(self.forceProfile['fBiasMag']) # compute desired force desiredForce = forceCtrlDir * fRefMag # get the current force forceError = self.fCurrent + desiredForce # apply admittance gain to compute motion desiredTwist = PyKDL.Twist() desiredTwist.vel = PyKDL.Vector(\ forceError.x()*self.forceProfile['admittanceGains'][0], forceError.y()*self.forceProfile['admittanceGains'][1], forceError.z()*self.forceProfile['admittanceGains'][2]) velNorm = desiredTwist.vel.Norm() if velNorm > self.resolvedRatesConfig['absVelMax']: desiredTwist.vel.Normalize() desiredTwist.vel = desiredTwist.vel \ * self.resolvedRatesConfig['absVelMax'] desiredTwist.rot = PyKDL.Vector(0.0, 0.0, 0.0) return desiredTwist
def impedStearing(T_B_Trd,imp_list,pt): # cart global move_time print("Rozpoczecie sterowania impendacyjnego---------------------------") # This test/tutorial executes simple impedance commands in Cartesian impedance mode. # To see the tool frame add 'tf' in rviz and enable 'right_arm_tool' frame. # At every state switch to cart_imp, the tool frames are reset. # Also, the tool impedance parameters are reset to 1500N/m in every # direction for linear stiffness and to 150Nm/rad in every direction for angular # stiffness, i.e. (1500,1500,1500,150,150,150)." """TEST WALNIECIA W SZAFKE""" t=countTime(T_B_Trd) actual_gripper_position = velma.getTf("B", "Tr") #aktualna pozycja chwytaka print("Moving right wrist to pose defined in world frame...") """Tak tutaj dalem 3 zamiast move_time*t bo dla malych ruchow wywalalo errora bo za maly czas wyliczalo""" if not velma.moveCartImpRight([T_B_Trd], [3], None, None, imp_list, [0.5], max_wrench=makeWrench([5,5,5], [5,5,5]), start_time=0.01, path_tol=PyKDL.Twist(PyKDL.Vector(pt,pt,pt), PyKDL.Vector(pt,pt,pt))): exitError(13) if velma.waitForEffectorRight() != 0: #zglaszane jak chwytak nie moze osiagnac zadanej pozycji return actual_gripper_position.p[0] , actual_gripper_position.p[1], actual_gripper_position.p[2] print("Pose reached, there was no collision") return actual_gripper_position.p[0] , actual_gripper_position.p[1], actual_gripper_position.p[2]
def load_manipulator_comd_config(self,config_data): # this func load configurations about the manipulator command that the user_interface gives # device frame offsets rot_X = ast.literal_eval(config_data.get( 'user_interface', 'TF_device2view_rot_X')) rot_Y = ast.literal_eval(config_data.get( 'user_interface', 'TF_device2view_rot_Y')) rot_Z = ast.literal_eval(config_data.get( 'user_interface', 'TF_device2view_rot_Z')) pos = ast.literal_eval(config_data.get( 'user_interface', 'TF_device2view_trans')) frame = PyKDL.Frame.Identity() frame.M = PyKDL.Rotation( rot_X[0],rot_Y[0],rot_Z[0], rot_X[1],rot_Y[1],rot_Z[1], rot_X[2],rot_Y[2],rot_Z[2]) frame.P = PyKDL.Vector(pos[0],pos[1],pos[2]) self.tf_dev2view = frame # command_type self.command_type = config_data.get( 'user_interface', 'command_type') # subscriber - manipulator command from user input rostopic_manipulator_comd = config_data.get( 'user_interface', 'rostopic_manipulator_comd') if (self.command_type=="pose"): self.manipulator_comd = [] rospy.Subscriber( rostopic_manipulator_comd, Pose, self.manipulator_comd_pose_CB) else: self.manipulator_comd = PyKDL.Twist() rospy.Subscriber( rostopic_manipulator_comd, Twist, self.manipulator_comd_twist_CB)
def inverse(self, t, TGB): if len(t) < self._num_jnts: rospy.logerr_throttle( 5, "[ServoIk.inverse] Wrong number of joints published. Published: {} Existing: {}" .format(len(t), self._num_jnts)) return None else: joints_array = kdl.JntArray(self._num_jnts) for i in xrange(self._num_jnts): joints_array[i] = t[i] joint_velocities_array = kdl.JntArray(self._num_jnts) FEB = kdl.Frame() FGB = kdl.Frame( kdl.Rotation.Quaternion(TGB[1][0], TGB[1][1], TGB[1][2], TGB[1][3]), kdl.Vector(TGB[0][0], TGB[0][1], TGB[0][2])) self._fk_p.JntToCart(joints_array, FEB) FEGB = FGB * FEB.Inverse() iterations = 0 while (FEGB.p.Norm() > LINEAR_TOLERANCE or FEGB.M.GetRotAngle()[0] > ANGULAR_TOLERANCE) \ and iterations < MAX_ITERATIONS: iterations += 1 twist = kdl.Twist(FEGB.p, FEGB.M.GetRot()) self._ik_v.CartToJnt(joints_array, twist, joint_velocities_array) maximum = 0.0 for i in xrange(self._num_jnts): if joint_velocities_array[i] > maximum: maximum = joint_velocities_array[i] if maximum > SATURATION: reduction = SATURATION / maximum else: reduction = 1.0 for i in xrange(self._num_jnts): joints_array[i] += reduction * joint_velocities_array[i] self._fk_p.JntToCart(joints_array, FEB) FEGB = FGB * FEB.Inverse() logdebug('amount of iterations needed: %s', iterations) if iterations >= MAX_ITERATIONS: return None solution = [] for i in xrange(self._num_jnts): while joints_array[i] < pi: joints_array[i] += 2.0 * pi while joints_array[i] > pi: joints_array[i] -= 2.0 * pi if joints_array[i] < self.lower_limits[i] \ or joints_array[i] > self.upper_limits[i]: return None solution.append(joints_array[i]) return solution
def impedMove(T_B_Trd, imp_list, pt, tm=3): # cart print "Rozpoczecie sterowania impendacyjnego---------------------------" # actual_gripper_position = velma.getTf("B", "Tr") # aktualna pozycja chwytaka print "Moving right wrist to pose defined in world frame..." my_wrench = makeWrench([5, 5, 5], [5, 5, 5]) my_twist = PyKDL.Twist(PyKDL.Vector(pt, pt, pt), PyKDL.Vector(pt, pt, pt)) rospy.sleep(0.5) print(len(imp_list)) """Tak tutaj dalem 3 zamiast move_time*t bo dla malych ruchow wywalalo errora bo za maly czas wyliczalo""" if not velma.moveCartImpRight([T_B_Trd], [tm], None, None, imp_list, [0.5], max_wrench=my_wrench, start_time=0.01, path_tol=my_twist): exitError(13) if velma.waitForEffectorRight( ) != 0: # zglaszane jak chwytak nie moze osiagnac zadanej pozycji print("I think it worked?") return #actual_gripper_position.p[0], actual_gripper_position.p[1], actual_gripper_position.p[2] print "Pose reached, there was no collision" return #actual_gripper_position.p[0], actual_gripper_position.p[1], actual_gripper_position.p[2]
def to_kdl_twist(self): vel = PyKDL.Vector(self._velocity['linear'][0], self._velocity['linear'][1], self._velocity['linear'][2]) rot = PyKDL.Vector(self._velocity['angular'][0], self._velocity['angular'][1], self._velocity['angular'][2]) return PyKDL.Twist(vel, rot)
def twist_to_kdl(twist): t = PyKDL.Twist() t.vel[0] = twist.linear.x t.vel[1] = twist.linear.y t.vel[2] = twist.linear.z t.rot[0] = twist.angular.x t.rot[1] = twist.angular.y t.rot[2] = twist.angular.z return t
def __init__(self): self.goal_traj = [] self.real_traj = [] self.acc_limit = PyKDL.Twist() self.acc_limit.vel[0] = 0.01 self.acc_limit.vel[1] = 0.01 self.acc_limit.rot[2] = 0.01 self.done = True urdf = rospy.get_param('robot_description') self.urdf = URDF.from_xml_string(hacky_urdf_parser_fix(urdf)) limits = PyKDL.Twist() limits.vel[0] = self.urdf.joint_map[x_joint].limit.velocity limits.vel[1] = self.urdf.joint_map[y_joint].limit.velocity limits.rot[2] = self.urdf.joint_map[z_joint].limit.velocity self._min_vel = -limits self._max_vel = limits self.cmd = None self.last_cmd = PyKDL.Twist() self.pose_history = None self.cmd_vel_sub = rospy.Publisher('~cmd_vel', Twist, queue_size=10) self.debug_vel = rospy.Publisher('debug_vel', Twist, queue_size=10) js = rospy.wait_for_message('/joint_states', JointState) self.x_index_js = js.name.index(x_joint) self.y_index_js = js.name.index(y_joint) self.z_index_js = js.name.index(z_joint) self.current_goal = None self.js_sub = rospy.Subscriber('/joint_states', JointState, self.js_cb, queue_size=10) self.state_pub = rospy.Publisher('{}/state'.format(name_space), JointTrajectoryControllerState, queue_size=10) self.server = SimpleActionServer( '{}/follow_joint_trajectory'.format(name_space), FollowJointTrajectoryAction, self.execute_cb, auto_start=False) self.server.start()
def twist_cb(self, msg): twist = PyKDL.Twist() twist[0] = msg.linear.x twist[1] = msg.linear.y twist[2] = msg.linear.z twist[3] = msg.angular.x twist[4] = msg.angular.y twist[5] = msg.angular.z self.twist = self._T_baseoffset_inverse * twist pass
def __init__(self, object_name, goal, goal_twist=kdl.Twist(), p_gain=1.0, d_gain=1.0): self._object_name = object_name self._goal_pose = goal self._goal_twist = goal_twist self._p_gain = p_gain self._d_gain = d_gain
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 __init__(self, name): pose_topic_name = name + 'pose' twist_topic_name = name + 'twist' button_topic_name = name + 'button' force_topic_name = name + 'force_feedback' self._active = False self._scale = 0.0002 self.pose = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0)) self.twist = PyKDL.Twist() # This offset is to align the pitch with the view frame R_off = Rotation.RPY(0.0, 0, 0) self._T_baseoffset = Frame(R_off, Vector(0, 0, 0)) self._T_baseoffset_inverse = self._T_baseoffset.Inverse() self._T_tipoffset = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0)) self.clutch_button_pressed = False # Used as Position Engage Clutch self.gripper_button_pressed = False # Used as Gripper Open Close Binary Angle self._force = DeviceFeedback() self._force.force.x = 0 self._force.force.y = 0 self._force.force.z = 0 self._force.position.x = 0 self._force.position.y = 0 self._force.position.z = 0 self._pose_sub = rospy.Subscriber(pose_topic_name, PoseStamped, self.pose_cb, queue_size=1) self._twist_sub = rospy.Subscriber(twist_topic_name, Twist, self.twist_cb, queue_size=1) ###### Commented self._button_sub = rospy.Subscriber(button_topic_name, DeviceButtonEvent, self.buttons_cb, queue_size=1) self._force_pub = rospy.Publisher(force_topic_name, DeviceFeedback, queue_size=1) self.switch_psm = False self._button_msg_time = rospy.Time.now() self._switch_psm_duration = rospy.Duration(0.5) print('Creating Geomagic Device Named: ', name, ' From ROS Topics') self._msg_counter = 0
def test_transform(self): b = tf2_ros.Buffer() t = TransformStamped() t.transform.translation.x = 1.0 t.transform.rotation = Quaternion(w=0.0, x=1.0, y=0.0, z=0.0) t.header.stamp = builtin_interfaces.msg.Time(sec=2) t.header.frame_id = 'a' t.child_frame_id = 'b' b.set_transform(t, 'eitan_rocks') out = b.lookup_transform('a', 'b', rclpy.time.Time(seconds=2), rclpy.time.Duration(seconds=2)) self.assertEqual(out.transform.translation.x, 1) self.assertEqual(out.transform.rotation.x, 1) self.assertEqual(out.header.frame_id, 'a') self.assertEqual(out.child_frame_id, 'b') v = PyKDL.Vector(1, 2, 3) out = b.transform( tf2_ros.Stamped(v, builtin_interfaces.msg.Time(sec=2), 'a'), 'b') self.assertEqual(out.x(), 0) self.assertEqual(out.y(), -2) self.assertEqual(out.z(), -3) f = PyKDL.Frame(PyKDL.Rotation.RPY(1, 2, 3), PyKDL.Vector(1, 2, 3)) out = b.transform( tf2_ros.Stamped(f, builtin_interfaces.msg.Time(sec=2), 'a'), 'b') self.assertEqual(out.p.x(), 0) self.assertEqual(out.p.y(), -2) self.assertEqual(out.p.z(), -3) # TODO(tfoote) check values of rotation t = PyKDL.Twist(PyKDL.Vector(1, 2, 3), PyKDL.Vector(4, 5, 6)) out = b.transform( tf2_ros.Stamped(t, builtin_interfaces.msg.Time(sec=2), 'a'), 'b') self.assertEqual(out.vel.x(), 1) self.assertEqual(out.vel.y(), -8) self.assertEqual(out.vel.z(), 2) self.assertEqual(out.rot.x(), 4) self.assertEqual(out.rot.y(), -5) self.assertEqual(out.rot.z(), -6) w = PyKDL.Wrench(PyKDL.Vector(1, 2, 3), PyKDL.Vector(4, 5, 6)) out = b.transform( tf2_ros.Stamped(w, builtin_interfaces.msg.Time(sec=2), 'a'), 'b') self.assertEqual(out.force.x(), 1) self.assertEqual(out.force.y(), -2) self.assertEqual(out.force.z(), -3) self.assertEqual(out.torque.x(), 4) self.assertEqual(out.torque.y(), -8) self.assertEqual(out.torque.z(), -4)
def resolvedRates(self, currentPose, desiredPose): # compute pose error (result in kdl.twist format) poseError = PyKDL.diff(currentPose, desiredPose) posErrNorm = poseError.vel.Norm() rotErrNorm = poseError.rot.Norm() # compute velocity magnitude based on position error norm if posErrNorm > self.resolvedRatesConfig['tolPos']: tolPosition = self.resolvedRatesConfig['tolPos'] lambdaVel = self.resolvedRatesConfig['velRatio'] if posErrNorm > (lambdaVel * tolPosition): velMag = self.resolvedRatesConfig['velMax'] else: velMax = self.resolvedRatesConfig['velMax'] velMin = self.resolvedRatesConfig['velMin'] velMag = (velMin \ + (posErrNorm - tolPosition) \ * (velMax-velMin) / (tolPosition * (lambdaVel-1))) else: velMag = 0.0 # compute angular velocity based on rotation error norm if rotErrNorm > self.resolvedRatesConfig['tolRot']: tolRotation = self.resolvedRatesConfig['tolRot'] lambdaRot = self.resolvedRatesConfig['rotRatio'] if rotErrNorm > (lambdaRot * tolRotation): angVelMag = self.resolvedRatesConfig['angVelMax'] else: angVelMax = self.resolvedRatesConfig['angVelMax'] angVelMin = self.resolvedRatesConfig['angVelMin'] angVelMag = (angVelMin \ + (rotErrNorm - tolRotation) \ * (angVelMax - angVelMin) \ / (tolRotation * (lambdaRot-1))) else: angVelMag = 0.0 # The resolved rates is implemented as Nabil Simaan's notes # apply both the velocity and angular velocity in the error pose direction desiredTwist = PyKDL.Twist() poseError.vel.Normalize() # normalize to have the velocity direction desiredTwist.vel = poseError.vel * velMag poseError.rot.Normalize() # normalize to have the ang vel direction desiredTwist.rot = poseError.rot * angVelMag # Check whether we have reached our goal # goalReached = desiredTwist.vel.Norm() <= self.resolvedRatesConfig['velMin'] \ # and desiredTwist.rot.Norm() <= self.resolvedRatesConfig['angVelMin'] # Ignore rotation for calculating goalReached! goalReached = desiredTwist.vel.Norm( ) <= self.resolvedRatesConfig['velMin'] return desiredTwist, goalReached
def test_transform(self): b = tf2_ros.Buffer() t = TransformStamped() t.transform.translation.x = 1 t.transform.rotation.x = 1 t.header.stamp = rospy.Time(2.0) t.header.frame_id = 'a' t.child_frame_id = 'b' b.set_transform(t, 'eitan_rocks') out = b.lookup_transform('a', 'b', rospy.Time(2.0), rospy.Duration(2.0)) self.assertEqual(out.transform.translation.x, 1) self.assertEqual(out.transform.rotation.x, 1) self.assertEqual(out.header.frame_id, 'a') self.assertEqual(out.child_frame_id, 'b') v = PyKDL.Vector(1, 2, 3) out = b.transform(tf2_ros.Stamped(v, rospy.Time(2), 'a'), 'b') self.assertEqual(out.x(), 0) self.assertEqual(out.y(), -2) self.assertEqual(out.z(), -3) f = PyKDL.Frame(PyKDL.Rotation.RPY(1, 2, 3), PyKDL.Vector(1, 2, 3)) out = b.transform(tf2_ros.Stamped(f, rospy.Time(2), 'a'), 'b') print(out) self.assertEqual(out.p.x(), 0) self.assertEqual(out.p.y(), -2) self.assertEqual(out.p.z(), -3) # TODO(tfoote) check values of rotation t = PyKDL.Twist(PyKDL.Vector(1, 2, 3), PyKDL.Vector(4, 5, 6)) out = b.transform(tf2_ros.Stamped(t, rospy.Time(2), 'a'), 'b') self.assertEqual(out.vel.x(), 1) self.assertEqual(out.vel.y(), -8) self.assertEqual(out.vel.z(), 2) self.assertEqual(out.rot.x(), 4) self.assertEqual(out.rot.y(), -5) self.assertEqual(out.rot.z(), -6) w = PyKDL.Wrench(PyKDL.Vector(1, 2, 3), PyKDL.Vector(4, 5, 6)) out = b.transform(tf2_ros.Stamped(w, rospy.Time(2), 'a'), 'b') self.assertEqual(out.force.x(), 1) self.assertEqual(out.force.y(), -2) self.assertEqual(out.force.z(), -3) self.assertEqual(out.torque.x(), 4) self.assertEqual(out.torque.y(), -8) self.assertEqual(out.torque.z(), -4)
def resolvedRates(config, currentPose, desiredPose): # compute pose error (result in kdl.twist format) poseError = PyKDL.diff(currentPose, desiredPose) posErrNorm = poseError.vel.Norm() rotErrNorm = poseError.rot.Norm() angVelMag = config['angVelMax'] if rotErrNorm < config['tolRot']: angVelMag = 0.0 elif rotErrNorm < angVelMag: angVelMag = rotErrNorm # compute velocity magnitude based on position error norm if posErrNorm > config['tolPos']: tolPosition = config['tolPos'] lambdaVel = config['velRatio'] if posErrNorm > (lambdaVel * tolPosition): velMag = config['velMax'] else: velMax = config['velMax'] velMin = config['velMin'] velMag = velMin + (posErrNorm - tolPosition) * \ (velMax - velMin)/(tolPosition*(lambdaVel-1)) else: velMag = 0.0 # compute angular velocity based on rotation error norm if rotErrNorm > config['tolRot']: tolRotation = config['tolRot'] lambdaRot = config['rotRatio'] if rotErrNorm > (lambdaRot * tolRotation): angVelMag = config['angVelMax'] else: angVelMax = config['angVelMax'] angVelMin = config['angVelMin'] angVelMag = angVelMin + (rotErrNorm - tolRotation) * \ (angVelMax - angVelMin)/(tolRotation*(lambdaRot-1)) else: angVelMag = 0.0 # The resolved rates is implemented as Nabil Simaan's notes # apply both the velocity and angular velocity in the error pose direction desiredTwist = PyKDL.Twist() poseError.vel.Normalize() # normalize to have the velocity direction desiredTwist.vel = poseError.vel * velMag poseError.rot.Normalize() # normalize to have the ang vel direction desiredTwist.rot = poseError.rot * angVelMag return desiredTwist
def moveInCartImpMode(velma, T_B_dest, tol=10): if not velma.moveCartImpRight( [T_B_dest], [5.0], None, None, None, None, PyKDL.Wrench(PyKDL.Vector(5, 5, 5), PyKDL.Vector(5, 5, 5)), start_time=0.1, path_tol=PyKDL.Twist(PyKDL.Vector(tol, tol, tol), PyKDL.Vector(tol, tol, tol))): exitError(8) if velma.waitForEffectorRight() != 0: if not velma.moveCartImpRightCurrentPos(start_time=0.01): exitError(9) return False else: return True
def get_qdot_from_twist(self, q, twist): """Returns the joint velocities for a given twist Arguments: q -- The current robot configuration twist -- The desired twist """ q_arr = kdl.JntArray(self._num_of_joints) qdot_arr = kdl.JntArray(self._num_of_joints) kdl_twist = kdl.Twist() for joint_num in range(self._num_of_joints): q_arr[joint_num] = q[joint_num] for joint_num in range(6): kdl_twist[joint_num] = twist[joint_num] self._tv_solver.CartToJnt(q_arr, kdl_twist, qdot_arr) ret = np.array([0.0] * self._num_of_joints) for joint_num in range(self._num_of_joints): ret[joint_num] = qdot_arr[joint_num] return ret
def _command_callback(self, msg): self._command = np.zeros(6) rot = self._trans.M twist = PyKDL.Twist( PyKDL.Vector(msg.twist.linear.x, msg.twist.linear.y, msg.twist.linear.z), PyKDL.Vector(msg.twist.angular.x, msg.twist.angular.y, msg.twist.angular.z)) twist = self._trans.M * twist self._command[0] = twist.vel.x() self._command[1] = twist.vel.y() self._command[2] = twist.vel.z() if not self._position_only: self._command[3] = twist.rot.x() self._command[4] = twist.rot.y() self._command[5] = twist.rot.z()
def resolvedRates(self): # get current and desired robot pose (desired is the top of queue) currentPose = self.robot.get_current_position() desiredPose = self.trajectory[0] # compute pose error (result in kdl.twist format) poseError = PyKDL.diff(currentPose, desiredPose) posErrNorm = poseError.vel.Norm() rotErrNorm = poseError.rot.Norm() # compute velocity magnitude based on position error norm if posErrNorm > self.resolvedRatesConfig['tolPos']: tolPosition = self.resolvedRatesConfig['tolPos'] lambdaVel = self.resolvedRatesConfig['velRatio'] if posErrNorm > (lambdaVel * tolPosition): velMag = self.resolvedRatesConfig['velMax'] else: velMax = self.resolvedRatesConfig['velMax'] velMin = self.resolvedRatesConfig['velMin'] velMag = velMin + \ (velMax - velMin)/(tolPosition*(lambdaVel-1)) else: velMag = 0.0 # compute angular velocity based on rotation error norm if rotErrNorm > self.resolvedRatesConfig['tolRot']: tolRotation = self.resolvedRatesConfig['tolRot'] lambdaRot = self.resolvedRatesConfig['rotRatio'] if posErrNorm > (lambdaRot * tolRotation): angVelMag = self.resolvedRatesConfig['angVelMax'] else: angVelMax = self.resolvedRatesConfig['angVelMax'] angVelMin = self.resolvedRatesConfig['angVelMin'] angVelMag = angVelMin + \ (angVelMax - angVelMin)/(tolRotation*(lambdaRot-1)) else: angVelMag = 0.0 # The resolved rates is implemented as Nabil Simaan's notes # apply both the velocity and angular velocity in the error pose direction sys_dt = self.resolvedRatesConfig['dt'] desiredTwist = PyKDL.Twist() poseError.vel.Normalize() # normalize to have the velocity direction desiredTwist.vel = poseError.vel * velMag * sys_dt poseError.rot.Normalize() # normalize to have the ang vel direction desiredTwist.rot = poseError.rot * angVelMag * sys_dt return desiredTwist
def _check_traj(self, dT): """Check if the end-effector has reached the desired position of target waypoint. """ _ee_position = self._get_ee_position() _targ_wp_position = tr.translation_from_matrix(self.traj_list[self.cur_wp_idx]) if self.cur_wp_idx < self.num_wp - 1: # indicate next waypoint self.cur_wp_idx += 1 self.traj_elapse += dT elif self.cur_wp_idx == self.num_wp - 1: # robot has reached the last waypoint # rospy.logwarn('Reached target object') self.cur_wp_idx = 0 self.isPathPlanned = False self.traj_elapse = 0 print ('Reset integral twist ...') self.integ_twist = PyKDL.Twist(PyKDL.Vector(0, 0, 0), PyKDL.Vector(0, 0, 0)) # initialize integral twist if np.linalg.norm(np.array(self.ee_position) - self._get_target_pose()) <= self.dist_threshold: rospy.logwarn('Reached target object') rospy.delete_param('vel_calc')
def move_frame(velma, frame, time_wrist=6, frame_tool=None, time_tool=None): print "Moving right wrist to pose defined in world frame..." # if not velma.moveCartImpRight([frame], [3.0], frame_tool_vector, time_tool_vector, None, None, PyKDL.Wrench(PyKDL.Vector(5,5,5), PyKDL.Vector(5,5,5)), start_time=0.5): twist = PyKDL.Twist(PyKDL.Vector(0.1, 0.1, 0.1), PyKDL.Vector(0.1, 0.1, 0.1)) wr = PyKDL.Wrench(PyKDL.Vector(80, 80, 500), PyKDL.Vector(120, 120, 120)) if not velma.moveCartImpRight([frame], [time_wrist], frame_tool, time_tool, [wr], [time_wrist], PyKDL.Wrench(PyKDL.Vector(5, 5, 5), PyKDL.Vector(5, 5, 5)), start_time=0.5, path_tol=twist): print "wait for three" exitError(13) try: if velma.waitForEffectorRight() != 0: print "wait for effector right" return True except: rospy.sleep(0.5)
def main(): b = tf2_ros.Buffer() t = TransformStamped() t.transform.translation.x = 1 t.transform.rotation.x = 1 t.header.stamp = rospy.Time(2.0) t.header.frame_id = 'a' t.child_frame_id = 'b' b.set_transform(t, 'eitan_rocks') print b.lookup_transform('a', 'b', rospy.Time(2.0), rospy.Duration(2.0)) v = PyKDL.Vector(1, 2, 3) print b.transform(tf2_ros.Stamped(v, rospy.Time(2), 'a'), 'b') f = PyKDL.Frame(PyKDL.Rotation.RPY(1, 2, 3), PyKDL.Vector(1, 2, 3)) print b.transform(tf2_ros.Stamped(f, rospy.Time(2), 'a'), 'b') t = PyKDL.Twist(PyKDL.Vector(1, 2, 3), PyKDL.Vector(4, 5, 6)) print b.transform(tf2_ros.Stamped(t, rospy.Time(2), 'a'), 'b') w = PyKDL.Wrench(PyKDL.Vector(1, 2, 3), PyKDL.Vector(4, 5, 6)) print b.transform(tf2_ros.Stamped(w, rospy.Time(2), 'a'), 'b')
def forceAdmittanceControl(self, forceCtrlDir): # compute the desired force magnitude sinamplitudeMag = np.sin((2 * np.pi)/self.forceProfile['period'] * time() ) \ * abs(self.forceProfile['amplitude'])+ abs(self.forceProfile['amplitude'])/2 fRefMag = sinamplitudeMag + abs(self.forceProfile['fBiasMag']) # compute desired force desiredForce = forceCtrlDir * fRefMag # get the current force forceError = self.fCurrent + desiredForce # apply admittance gain to compute motion desiredTwist = PyKDL.Twist() desiredTwist.vel = PyKDL.Vector(\ forceError.x()*self.forceProfile['admittanceGains'][0], forceError.y()*self.forceProfile['admittanceGains'][1], forceError.z()*self.forceProfile['admittanceGains'][2]) velNorm = desiredTwist.vel.Norm() if velNorm > self.resolvedRatesConfig['velMax']: desiredTwist.vel.Normalize() desiredTwist.vel = desiredTwist.vel \ * self.resolvedRatesConfig['velMax'] desiredTwist.rot = PyKDL.Vector(0.0, 0.0, 0.0) return desiredTwist
def leaveCabinet(): print "Oddalenie od szafki..." Goal = PyKDL.Vector(1 / 3 * doorL_x + 2 / 3 * doorR_x - 0.4, 1 / 3 * doorL_y + 2 / 3 * doorR_y - 0.2, doorR_z + 0.1 - 0.25) T_B_Door = PyKDL.Frame(PyKDL.Rotation.RotZ(0), Goal) if not velma.moveCartImpRight( [T_B_Door], [5.0], None, None, None, None, PyKDL.Wrench(PyKDL.Vector(100, 100, 0.7), PyKDL.Vector(0.7, 0.7, 0.7)), start_time=0.5, path_tol=PyKDL.Twist(PyKDL.Vector(1, 1, 1), PyKDL.Vector(1, 1, 1))): exitError(8) if velma.waitForEffectorRight() != 0: exitError(9) rospy.sleep(0.5)
def compute_pose_velocity_cartesian_eular(joint, joint_vel): # computes the pose and velocity (twist) based on cartesian and euler # the default eular sequence used is ZYX # compute the pose (position and orientation) pos = PyKDL.Vector(joint[0], joint[1], joint[2]) rot = PyKDL.Rotation.EulerZYX(joint[3], joint[4], joint[5]) pose = PyKDL.Frame(rot, pos) # compute the twist (velocity) lin_vel = PyKDL.Vector(joint_vel[0], joint_vel[1], joint_vel[2]) rotation_local = PyKDL.Rotation.Identity() ang_vel = PyKDL.Vector() for i in range(3, 6): if (i == 3): det_rot = joint_vel[i] * rotation_local.UnitZ() rotation_local = rotation_local * PyKDL.Rotation.RotZ(joint[i]) elif (i == 4): det_rot = joint_vel[i] * rotation_local.UnitY() rotation_local = rotation_local * PyKDL.Rotation.RotY(joint[i]) elif (i == 5): det_rot = joint_vel[i] * rotation_local.UnitX() ang_vel = ang_vel + det_rot twist = PyKDL.Twist(lin_vel, ang_vel) return pose, twist