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