def callbacallback(twist_msg): m3 = twist_msg.linear.x m0 = twist_msg.linear.y m1 = twist_msg.linear.z m2 = twist_msg.angular.z # PWM M = np.array([m0, m1, m2, m3]) # Motor saturation M[M > pwm_max] = pwm_max # M[M<=pwm_min] = 0 # Real robot does not generage forces with less than this # pwm to forces (in grams) MF = np.power((M / pwm_max - c1) / c2, 2) - c3 # forces in kilograms MF /= 1000 MF[MF < 0] = 0. # compute forces [fx, fy, mz] = np.dot(A, MF) # publish twist msg = Wrench() force = Vector3() torque = Vector3() force.x = fx force.y = fy torque.z = mz msg.force = force msg.torque = torque pub1.publish(msg)
def point_callback(self, msg): x = msg.x y = msg.y z = msg.z curr_point = np.array([x,y,z]) #cm p_err = (curr_point - self.ref_point)*(1/np.power(10, 2)) #m if self.prev_err is None: self.prev_err = p_err d_err = (p_err - self.prev_err) #m if (np.greater(p_err, (self.ref_point - self.ref_error)).all() and np.less(p_err, (self.ref_point + self.ref_error)).all() and not has_reached_point ): self.ref_point = curr_point self.has_reached_point = True else: force = self.Kp*p_err + self.Kd*d_err force_vector = Vector3() force_vector.x = force[0] force_vector.y = force[1] force_vector.z = force[2] wrench_msg = Wrench() wrench_msg.force = force_vector self.gazebo_force_pub.publish(wrench_msg) self.prev_err = p_err
def publishWrench(data, hand): wr = data.estimated_external_wrench wrench_msg = Wrench() wrench_msg.force = Vector3(wr.x, wr.y, wr.z) wrench_msg.torque = Vector3(wr.c, wr.b, wr.a) wrench_stamped_msg = WrenchStamped() wrench_stamped_msg.wrench = wrench_msg wrench_stamped_msg.header.stamp = rospy.Time(0) wrench_stamped_msg.header.frame_id = hand_to_frame[hand] hand_wrench_pub[hand].publish(wrench_stamped_msg)
def to_wrench(array): """ Converts a numpy array into a C{geometry_msgs/Wrench} ROS message. @type array: np.array @param array: The wrench as numpy array @rtype: geometry_msgs/Wrench @return: The resulting ROS message """ msg = Wrench() msg.force = to_vector3(array[:3]) msg.torque = to_vector3(array[3:]) return msg
def get_thruster_manager_message(fx=1, fy=1, fz=1, tx=1, ty=1, tz=1): force = Vector3() force.x = fx force.y = fy force.z = fz torque = Vector3() torque.x = tx torque.y = ty torque.z = tz msg = Wrench() msg.force = force msg.torque = torque return msg
def set_torque(self, nx, ny, nz): """ Set the Torque for of this object in parent frame. If a previous Wrench command had been set, the force from that command will be used :param nx: :param ny: :param nz: :return: """ _wrench_cmd = Wrench() _wrench_cmd.force = self.get_force_command() _wrench_cmd.torque.x = nx _wrench_cmd.torque.y = ny _wrench_cmd.torque.z = nz self.set_wrench(_wrench_cmd)
def to_wrench(array): """ Convert a numpy array into a `geometry_msgs/Wrench` ROS message. Parameters ---------- array: array_like The wrench :math:`[f_x, f_y, f_z, t_x, t_y, t_z]` as numpy array Returns ------- msg: geometry_msgs/Wrench The resulting ROS message """ msg = Wrench() msg.force = to_vector3(array[:3]) msg.torque = to_vector3(array[3:]) return msg
def multi_dof_joint_state(cls, msg, obj): obj.header = decode.header(msg, obj.header, "") obj.joint_names = msg["joint_names"] for i in msg['_length_trans']: trans = Transform() trans.translation = decode.translation(msg, trans.translation, i) trans.rotation = decode.rotation(msg, trans.rotation, i) obj.transforms.append(trans) for i in msg['_length_twist']: tw = Twist() tw.linear = decode.linear(msg, tw.linear, i) tw.angular = decode.angular(msg, tw.angular, i) obj.twist.append(twist) for i in msg['_length_twist']: wr = Wrench() wr.force = decode.force(msg, wr.force, i) wr.torque = decode.torque(msg, wr.torque, i) obj.wrench.append(wr) return(obj)
def show_gazebo_models(self): try: # model_coordinates = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState) # resp_coordinates = model_coordinates('my_robot','left_front_wheel') # print(resp_coordinates) apply_wrench = rospy.ServiceProxy('/gazebo/apply_body_wrench', ApplyBodyWrench) wrench = Wrench() wrench.force = Vector3(50,50,50) wrench.torque = Vector3(50,50,50) rate = rospy.Rate(100) success = apply_wrench( 'my_robot', 'world', Point(0, 0, 0), wrench, rospy.Time().now(), rospy.Duration(5)) except rospy.ServiceException as e: rospy.loginfo("Get Model State service call failed: {0}".format(e))
try: apply_wrench = rospy.ServiceProxy('/gazebo/apply_body_wrench', ApplyBodyWrench) except rospy.ServiceException as e: print('Service call failed, error=', e) sys.exit(-1) ns = rospy.get_namespace().replace('/', '') body_name = '%s/base_link' % ns if starting_time >= 0: rate = rospy.Rate(100) while rospy.get_time() < starting_time: rate.sleep() wrench = Wrench() wrench.force = Vector3(*force) wrench.torque = Vector3(*torque) success = apply_wrench(body_name, 'world', Point(0, 0, 0), wrench, rospy.Time().now(), rospy.Duration(duration)) if success: print('Body wrench perturbation applied!') print('\tFrame: ', body_name) print('\tDuration [s]: ', duration) print('\tForce [N]: ', force) print('\tTorque [Nm]: ', torque) else: print('Failed!')
apply_wrench = rospy.ServiceProxy('/gazebo/apply_body_wrench', ApplyBodyWrench) except rospy.ServiceException as e: print('Service call failed, error=', e) sys.exit(-1) ns = rospy.get_namespace().replace('/', '') body_name = '%s/base_link' % ns if starting_time >= 0: rate = rospy.Rate(100) while rospy.get_time() < starting_time: rate.sleep() wrench = Wrench() wrench.force = Vector3(*force) wrench.torque = Vector3(*torque) success = apply_wrench( body_name, 'world', Point(0, 0, 0), wrench, rospy.Time().now(), rospy.Duration(duration)) if success: print('Body wrench perturbation applied!') print('\tFrame: ', body_name) print('\tDuration [s]: ', duration) print('\tForce [N]: ', force) print('\tTorque [Nm]: ', torque)
def navigate(self, x, y): wrench = Wrench() #rate set to 100 to send messages to skibot at a specific interval rate = rospy.Rate(100) while not rospy.is_shutdown(): wrench.force = Vector3() wrench.torque = Vector3() wrench.force.x = 0 wrench.torque.z = 0 #Check if pose is None because pose_callback may not have been called yet if self.pose != None: self.x_error = x - self.pose.x self.y_error = y - self.pose.y # If the absolute combined error is greater than acceptable apply force to skibot if np.abs(self.x_error) + np.abs(self.y_error) > 0.01: #Calculates the goalAngle using a tan inverse function built into numpy goalAngle = np.arctan2((y - self.pose.y), (x - self.pose.x)) * 180 / np.pi #Calculate the error between current theta and goalAngle self.angle_error = (goalAngle - (self.pose.theta * 180 / np.pi)) % 360 if self.angle_error >= 180: self.angle_error -= 360 #The calculated angle is off by 45 degrees to this math corrects this and accounts for negative/positive flip if self.angle_error < -135: self.angle_error = 180 - ( (180 + (self.angle_error - 45) + 1) * -1) if self.angle_error < -135: self.angle_error = 180 - ( (180 + (self.angle_error - 45) + 1) * -1) if self.angle_error > 0.5 and self.pose != None: if self.pose.theta_velocity < 1: #Apply torque based on angle error wrench.torque.z = self.angle_error * 0.05 if self.angle_error < -0.5 and self.pose != None: if self.pose.theta_velocity > -1: #Apply torque based on angle error wrench.torque.z = self.angle_error * 0.05 # if the current total velocity is less than the total error if np.abs(self.pose.x_velocity) + np.abs( self.pose.y_velocity) < (np.abs(self.x_error) + np.abs(self.y_error)): #force is determined by the total absolute error and current absolute velocity wrench.force.x = ((np.abs(self.x_error) + np.abs(self.y_error))) - np.abs( self.pose.x_velocity) + np.abs( self.pose.y_velocity) # if the current total velocity is greater than the total error if np.abs(self.pose.x_velocity) + np.abs( self.pose.y_velocity) > (np.abs(self.x_error) + np.abs(self.y_error)): #force is determined by the total absolute error and current absolute velocity wrench.force.x = -0.3 * np.abs( self.pose.x_velocity) + np.abs( self.pose.y_velocity) - ( (np.abs(self.x_error) + np.abs(self.y_error))) #clips the acceleration force to ensure movement even when there is minimal error if wrench.force.x < 0: wrench.force.x = np.clip(wrench.force.x, -.5, -.35) if wrench.force.x > 0: wrench.force.x = np.clip(wrench.force.x, .35, .5) countdown = 0 else: #on the target so begin to countdown self.countdown += 1 if self.countdown > 1500: self.countdown = 0 #if the countdown has passed 1500 then the service is completed return True self.vel_pub.publish(wrench)