def main():
if MORSE == True:
rospy.init_node('pendulum_brain', anonymous=True)
pub = rospy.Publisher('pendulum/control', Wrench)
r = rospy.Rate(10) # 10 Hz
# TODO: make this position and velocity
sub = rospy.Subscriber('pendulum/motion', Odometry, odom_callback)
while not rospy.is_shutdown():
torque = u.origin['X'].decoded_output.get_value()[0]
torque_msg = Wrench()
torque_msg.torque.x = torque * 42
pub.publish( torque_msg )
net.run(0.01) # TODO: step the right amount of time
r.sleep()
else: #Gazebo
rospy.init_node('pendulum_brain', anonymous=True)
pub = rospy.Publisher('pendulum/torque', Float64)
r = rospy.Rate(10) # 10 Hz
# TODO: make this position and velocity
sub = rospy.Subscriber('pendulum/motion', Vector3, motion_callback)
while not rospy.is_shutdown():
torque = u.origin['X'].decoded_output.get_value()[0]
torque_msg = Float64()
torque_msg.data = torque * 2337
pub.publish( torque_msg )
net.run(0.01) # TODO: step the right amount of time
r.sleep()
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 _on_press(self, key):
"""
Handles key presses
@param key: They character of the key pressed
"""
msg = Wrench()
if key == keyboard.Key.up:
msg.torque.x = -self.torque_mags[0]
elif key == keyboard.Key.down:
msg.torque.x = self.torque_mags[0]
elif key == keyboard.Key.left:
msg.torque.z = self.torque_mags[2]
elif key == keyboard.Key.right:
msg.torque.z = -self.torque_mags[2]
elif 'char' in dir(key):
if key.char == 'w':
msg.force.x = self.force_mags[0]
elif key.char == 's':
msg.force.x = -self.force_mags[0]
elif key.char == 'a':
msg.force.y = self.force_mags[1]
elif key.char == 'd':
msg.force.y = -self.force_mags[1]
elif key.char == 'q':
msg.force.z = self.force_mags[2]
elif key.char == 'z':
msg.force.z = -self.force_mags[2]
self.force_pub.publish(msg)
def update_callback(event):
global desired_state, desired_state_dot, state, stat_dot, state_dot_body, previous_error, enable
lock.acquire()
#print 'desired state', desired_state
#print 'current_state', state
def smallest_coterminal_angle(x):
return (x + math.pi) % (2 * math.pi) - math.pi
# sub pd-controller sans rise
e = numpy.concatenate([
desired_state[0:3] - state[0:3],
map(smallest_coterminal_angle, desired_state[3:6] - state[3:6])
]) # e_1 in paper
vbd = _jacobian_inv(state).dot(K.dot(e) + desired_state_dot)
e2 = vbd - state_dot_body
output = Ks.dot(e2)
#print 'output',output
lock.release()
if (not (odom_active)):
output = [0, 0, 0, 0, 0, 0]
if (enable):
controller_wrench.publish(
WrenchStamped(header=Header(
stamp=rospy.Time.now(),
frame_id="/base_link",
),
wrench=Wrench(
force=Vector3(x=output[0], y=output[1], z=0),
torque=Vector3(x=0, y=0, z=output[5]),
)))
def _on_release(self, key):
"""
Handles key releases
@param key: They character of the key released
"""
msg = Wrench()
if key == keyboard.Key.up:
msg.torque.x = 0.0
elif key == keyboard.Key.down:
msg.torque.x = 0.0
elif key == keyboard.Key.left:
msg.torque.z = 0.0
elif key == keyboard.Key.right:
msg.torque.z = 0.0
elif 'char' in dir(key):
if key.char == 'w':
msg.force.x = 0.0
elif key.char == 's':
msg.force.x = 0.0
elif key.char == 'a':
msg.force.y = 0.0
elif key.char == 'd':
msg.force.y = 0.0
elif key.char == 'q':
msg.force.z = 0.0
elif key.char == 'z':
msg.force.z = 0.0
self.force_pub.publish(msg)
def move_joints(side):
limb = intera_interface.Limb(side)
joints = limb.joint_names()
# Publish end-effector torques to a topic
pub_torque = rospy.Publisher('endpoint_torque', Wrench, queue_size = 10)
action_count = 0; i = 0;
while not rospy.is_shutdown():
force = limb.endpoint_effort()['force']
torque = limb.endpoint_effort()['torque']
w = Wrench(Vector3(force.x, force.y, force.z), Vector3(torque.x, torque.y, torque.z));
pub_torque.publish(w);
action_count += 1;
if action_count % 10000 != 0:
continue
else:
action_count = 0
# Move joints according to specified locations
print(force)
i += 1;
if (i >= len(default_positions)):
i = 0;
raw_input("Press Enter to continue..." + str(i))
limb.set_joint_positions(default_positions[i]);
diff = sum([(limb.joint_angle(key) - default_positions[i][key])** 2 for key in default_positions[i]])
while (diff > 0.0001):
limb.set_joint_positions(default_positions[i])
diff = sum([(limb.joint_angle(key) - default_positions[i][key])** 2 for key in default_positions[i]])
time.sleep(0.005)
def update(self, timestamp, pose, angular_velocity, linear_acceleration):
# Prepare state vector
#state = np.array([
# pose.position.x, pose.position.y, pose.position.z,
# pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w])
pos = np.array([pose.position.x, pose.position.y, pose.position.z])
#lin_acc = np.array([linear_acceleration.x,linear_acceleration.y,linear_acceleration.z])
self.position_que.append(pos)
self.time_que.append(timestamp)
delta_t = self.time_que[1] - self.time_que[0]
delta_s = self.position_que[1] - self.position_que[0]
if delta_t == 0.0:
vel = self.zero_vel
else:
vel = delta_s / delta_t
state = np.concatenate((pos, vel, self.target), axis=-1)
# Compute reward / penalty and check if this episode is complete
done = False
distance = np.linalg.norm(state[:3] - self.target)
#reward = -min(distance,20.)
reward = -10.0 + ((30. / (1. + distance)))
#if pos[2] > 1.0 and pos[2] < 15.0:
# reward -= 0.01* np.linalg.norm(vel)
#else:
# reward -=5.0
#reward = (8. - distance)*0.5
if timestamp > self.max_duration:
done = True
# Take one RL step, passing in current state and reward, and obtain action
# Note: The reward passed in here is the result of past action(s)
action = self.agent.step(state, reward,
done) # note: action = <force; torque> vector
# Convert to proper force command (a Wrench object) and return it
if action is not None:
action = np.clip(action.flatten(), self.action_space.low,
self.action_space.high
) # flatten, clamp to action space limits
return Wrench(force=Vector3(action[0], action[1], action[2]),
torque=Vector3(action[3], action[4],
action[5])), done
else:
return Wrench(), done
def talker():
pub1 = rospy.Publisher('ati_ft_data', Wrench, queue_size=1)
pub2 = rospy.Publisher('ati_ft_data1', Wrench, queue_size=1)
rospy.init_node('ati_ft', anonymous=True)
#Open the socket to receive F/T data
HOST = '192.168.99.120' # The remote host
PORT = 49389 # The same port as used by the server
input_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
#time.sleep(2)
input_socket.connect((HOST, PORT))
input_fhandle = input_socket.makefile('r')
#Identify the columns we want to output
header = input_fhandle.readline()
header = header.split(',')
header = [column.strip() for column in header]
output_cols = [header.index(name) for name in OUTPUT_NAMES]
print(header)
print(output_cols)
#Publish data to the topic
#rate = rospy.Rate(150)
while not rospy.is_shutdown():
input_str = input_fhandle.readline()
input_str = input_str.split(',')
input_str = [column.strip() for column in input_str]
try:
wrench_obj = Wrench()
wrench_obj.force.x = float(input_str[output_cols[1]])
wrench_obj.force.y = float(input_str[output_cols[2]])
wrench_obj.force.z = float(input_str[output_cols[3]])
wrench_obj.torque.x = float(input_str[output_cols[4]])
wrench_obj.torque.y = float(input_str[output_cols[5]])
wrench_obj.torque.z = float(input_str[output_cols[6]])
wrench_obj2 = Wrench()
wrench_obj2.force.x = float(input_str[output_cols[7]])
wrench_obj2.force.y = float(input_str[output_cols[8]])
wrench_obj2.force.z = float(input_str[output_cols[9]])
wrench_obj2.torque.x = float(input_str[output_cols[10]])
wrench_obj2.torque.y = float(input_str[output_cols[11]])
wrench_obj2.torque.z = float(input_str[output_cols[12]])
pub1.publish(wrench_obj)
pub2.publish(wrench_obj2)
#rate.sleep()
except(Error):
print('An unknown read error occurred - discarding this F/T sample')
def test_lose_thruster(self):
'''Trigger a thruster loss and verify that it is no
longer used
Behavior:
1. Trigger the thruster failure alarm (By manually failing a simulated thruster)
2. Issue a wrench command
3. Verify that the navigator_msgs/Thrust command does not contain a command to that thruster
'''
rospy.Subscriber("/thrusters/thrust", Thrust, self.thrust_callback)
wrench_pub = rospy.Publisher('/wrench',
WrenchStamped,
queue_size=1,
latch=True)
thruster_to_fail = 'BRV'
# TODO: Compress this anymore
subscribed = wait_for_subscriber('thruster_mapper',
'wrench',
timeout=10.0)
self.assertTrue(
subscribed,
"{} did not not come up in time".format('thruster_mapper'))
# Fail a thruster
# TODO: Raise if waiting fails
rospy.wait_for_service('fail_thruster', timeout=5.0)
fail_thruster_proxy = rospy.ServiceProxy('fail_thruster', FailThruster)
fail_thruster_proxy(thruster_to_fail) # This will raise an alarm!
rospy.sleep(0.2) # Wait some time for the update to finish
# Spend up to 0.2 seconds waiting for a response (Bide time until the mapper finishes reacting)
# Looping because mapper ignores wrenches until it finishes remapping thrusters
give_up_time = time.time() + 0.2
while not (rospy.is_shutdown() and (time.time() < give_up_time)):
wrench_pub.publish(
WrenchStamped(
# The actual force/torque is irrelevant, we still send a 0 command for every active thruster
wrench=Wrench(force=Vector3(10.0, 10.0, 10.0),
torque=Vector3(10.0, 10.0, 10.0))))
if self.called:
break
time.sleep(0.1)
self.assertTrue(self.called)
self.assertIsNotNone(
self.last_thrust_cmd,
msg="Never got thrust command after issuing wrench")
names = []
for cmd in self.last_thrust_cmd.thruster_commands:
names.append(cmd.name)
self.assertNotIn(
thruster_to_fail,
names,
msg=
"Thruster mapper continued to issue commands to {} after thruster loss alarm"
.format(thruster_to_fail))
def make_shared_wrench(self):
msg = Wrench()
msg.force.y = self.force
msg.force.z = 0.5 #Constante para simular el usuario
msg.torque.y = 0.5 #Constante para simular el usuario
msg.torque.z = self.virtual_trq + self.torque
self.pub_shared_wrench.publish(msg)
return
def update(self, timestamp, pose, angular_velocity, linear_acceleration):
# Prepare state vector (pose only; ignore angular_velocity, linear_acceleration)
state = np.array([
pose.position.x, pose.position.y, pose.position.z,
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
])
# Compute reward / penalty and check if this episode is complete
done = False
reward = -min(
abs(self.target_z - pose.position.z), 20.0
) #1 - .1*(np.sum(abs(pose.position.z - self.target_z)))**2 # #
# print(reward)
# reward = zero for matching target z, -ve as you go farther, upto -20
if pose.position.z >= self.target_z: # agent has crossed the target height
print(self.target_z - pose.position.z)
reward -= -abs(
self.target_z - pose.position.z) #10.0 # bonus reward
print(reward)
done = True
# if pose.position.z < self.target_z:
# reward -= 10.0
if timestamp > self.max_duration: # agent has run out of time
reward -= 10.0 # extra penalty
done = True
# Take one RL step, passing in current state and reward, and obtain action
# Note: The reward passed in here is the result of past action(s)
# if not done:
# done = True
action = self.agent.step(state, reward,
done) # note: action = <force; torque> vector
# Convert to proper force command (a Wrench object) and return it
if action is not None:
action = np.clip(action.flatten(), self.action_space.low,
self.action_space.high
) # flatten, clamp to action space limits
return Wrench(force=Vector3(action[0], action[1], action[2]),
torque=Vector3(action[3], action[4],
action[5])), done
else:
return Wrench(), done
def __init__(self, **kwargs):
assert all('_' + key in self.__slots__ for key in kwargs.keys()), \
'Invalid arguments passed to constructor: %s' % \
', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__))
from std_msgs.msg import Header
self.header = kwargs.get('header', Header())
from geometry_msgs.msg import Wrench
self.wrench = kwargs.get('wrench', Wrench())
def callback(self, config, level):
msg = Wrench()
msg.force.z = config.F
msg.torque.x = config.Tx
msg.torque.y = config.Ty
msg.torque.z = config.Tz
self.wrench_pub.publish(msg)
return config
def update(self, timestamp, pose, angular_velocity, linear_acceleration):
# Prepare state vector
position = np.array([pose.position.x, pose.position.y, pose.position.z])
orientation = np.array([pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w])
if self.last_timestamp is None:
velocity = np.array([0.0, 0.0, 0.0])
else:
velocity = (position - self.last_position) / max(timestamp - self.last_timestamp, 1e-03) # prevent divide by zero
state = np.concatenate([position, orientation, velocity]) # combined state vector
self.last_timestamp = timestamp
self.last_position = position
# Track the deviations for position, orientation and veloctiy.
error_position = np.linalg.norm(self.target_position - state[0:3]) #Euclidian distance from target position
error_orientation = np.linalg.norm(self.target_orientation - state[3:7]) #Euclidian distance from target orientation
error_velocity = np.linalg.norm(self.target_velocity - state[7:10]) #Euclidian distance from target velocity
# Compute reward / penalty and check if this episode is complete
done = False
# Basic penalty
reward = -(self.weight_position * error_position + self.weight_orientation * error_orientation + self.weight_velocity * error_velocity)
# Extra penalty for z-axis
z_axis_penalty = (self.target_position[2] - pose.position.z)**2 + abs(self.target_velocity[2]-state[9])
reward -= z_axis_penalty
if error_position > self.max_error_position:
reward -= 50 * np.sqrt(np.exp(abs(self.max_duration-timestamp))) # extra penalty, agent strayed too far
done = True
elif timestamp > self.max_duration:
reward += 100.0 # extra reward, agent made it to the end
done = True
else:
# extra reward, agent hovers around target position
# weighted by difference between max_error_position and error_position
reward += (30.0 * np.log(self.max_error_position - error_position))
action = self.agent.step(state, reward, done)
if action is not None:
action = np.clip(action.flatten(), self.action_space.low, self.action_space.high) # flatten, clamp to action space limits
return Wrench(
force=Vector3(action[0], action[1], action[2]),
torque=Vector3(action[3], action[4], action[5])
), done
else:
return Wrench(), done
def update(self, timestamp, pose, angular_velocity, linear_acceleration):
# Prepare state vector (pose only; ignore angular_velocity, linear_acceleration)
state = np.array([
pose.position.x, pose.position.y, pose.position.z,
pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w])
# Compute reward / penalty and check if this episode is complete
done = False
reward = 0
if self.agent is not None and isinstance(self.agent, DQN_Agent):
if self.mode==-1:
self.agent.set_takeoff_mode()
self.mode = 0
if self.mode==0 and pose.position.z>=self.target_z:
self.agent.set_hover_mode()
self.hover_poss = pose.position.z
self.mode = 1
if self.mode==1 and timestamp>=self.time_per_step*2:
self.agent.set_land_mode()
self.mode = 2
if self.mode==0: # reward up movement when taking off
reward += pose.position.z/10.0
if self.mode==1: # reward staying calm when hovering
reward += abs(pose.position.z-self.hover_poss)/10.0
if self.mode==2: # reward moving down when landing
reward -= pose.position.z/10.0
if timestamp > self.max_duration: # agent has run out of time
done = True
# Take one RL step, passing in current state and reward, and obtain action
# Note: The reward passed in here is the result of past action(s)
action = self.agent.step(state, reward, done) # note: action = <force; torque> vector
# Convert to proper force command (a Wrench object) and return it
if action is not None:
action = np.clip(action.flatten(), self.action_space.low, self.action_space.high) # flatten, clamp to action space limits
return Wrench(
force=Vector3(action[0], action[1], action[2]),
torque=Vector3(action[3], action[4], action[5])
), done
else:
return Wrench(), done
def update(self, timestamp, pose, angular_velocity, linear_acceleration):
# Prepare state vector (pose only; ignore angular_velocity, linear_acceleration)
position = np.array([pose.position.x, pose.position.y, pose.position.z])
orientation = np.array([pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w])
if self.last_timestamp is None:
velocity = np.array([0.0, 0.0, 0.0])
else:
velocity = (position - self.last_position) / max(timestamp - self.last_timestamp, 1e-03) # prevent divide by zero
state = np.concatenate([position, orientation, velocity]) # combined state vector
self.last_timestamp = timestamp
self.last_position = position
state = np.array([
pose.position.x, pose.position.y, pose.position.z,
pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w])
# Compute reward / penalty and check if this episode is complete
done = False
reward = -min(abs(pose.position.z - self.target_z), 20.0) # reward = zero for matching target z, -ve as you go farther, upto -20
if pose.position.z == self.target_z: # agent has crossed the target height
reward += 10.0 # bonus reward
print('Bonus')
done = True
elif timestamp > self.max_duration: # agent has run out of time
reward -= 10.0 # extra penalty
print('Penalty')
done = True
# Take one RL step, passing in current state and reward, and obtain action
# Note: The reward passed in here is the result of past action(s)
action = self.agent.step(state, reward, done) # note: action = <force; torque> vector
# Convert to proper force command (a Wrench object) and return it
if action is not None:
action = np.clip(action.flatten(), self.action_space.low, self.action_space.high) # flatten, clamp to action space limits
return Wrench(
force=Vector3(action[0], action[1], action[2]),
torque=Vector3(action[3], action[4], action[5])
), done
else:
return Wrench(), done
def update(self, timestamp, pose, angular_velocity, linear_acceleration):
# Prepare state vector (pose only; ignore angular_velocity, linear_acceleration)
position = np.array([pose.position.x, pose.position.y, pose.position.z])
orientation = np.array([pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w])
# Calculate velocity
if self.last_timestamp is None:
velocity = np.array([0.0, 0.0, 0.0])
else:
velocity = (position - self.last_position) / max(timestamp - self.last_timestamp, 1e-03)
# Create state space and update lag variables
state = np.concatenate([position, orientation, velocity])
self.last_timestamp = timestamp
self.last_position = position
# Compute reward / penalty and check if this episode is complete
done = False
error_position = np.linalg.norm(self.target_position - state[0:3])
error_orientation = np.linalg.norm(self.target_orientation - state[3:7])
error_velocity = np.linalg.norm(self.target_velocity - state[7:10])
reward = - (self.position_weight * error_position +
self.orientation_weight * error_orientation +
self.velocity_weight * error_velocity)
if error_position > self.max_error_position:
reward -= 50.0
done = True
elif timestamp > self.max_duration:
reward += 50.0
done = True
# Take one RL step, passing in current state and reward, and obtain action
# Note: The reward passed in here is the result of past action(s)
action = self.agent.step(state, reward, done) # note: action = <force; torque> vector
# Convert to proper force command (a Wrench object) and return it
if action is not None:
action = np.clip(action.flatten(), self.action_space.low,
self.action_space.high) # flatten, clamp to action space limits
return Wrench(
force=Vector3(action[0], action[1], action[2]),
torque=Vector3(action[3], action[4], action[5])
), done
else:
return Wrench(), done
def joy_update(self, data):
"""callback for /joy
* sends commands to the simulation
Args:
data (Joy): the joystick's state
Left stick - right left - data.axes[0]
Left stick - up down - data.axes[1]
Button - 'X' - data.buttons[2]
"""
# print("joy_update: {:.2f}".format(data.axes[1]))
if (data.axes[0] != 0 or data.axes[1] != 0 or data.axes[4] != 0):
# self.robot.move_head(thetaY=data.axes[1], thetaZ=data.axes[0], forward=data.axes[4])
# self.robot.move_head(thetaY=data.axes[1], thetaZ=data.axes[0], forward=data.axes[4])
self.robot.move_head(thetaY=data.axes[1],
thetaZ=data.axes[0],
forward=data.axes[4])
CMD = MultiDOFJointState()
trans_i = Transform()
trans_i.translation.x = self.robot.joint_cmd[0]
twist_i = Twist()
wrench_i = Wrench()
CMD.transforms = [trans_i]
CMD.twist = [twist_i]
CMD.wrench = [wrench_i]
for i in range(self._link_N):
trans_i = Transform()
trans_i.rotation.z = self.robot.joint_ang[i * 2]
trans_i.rotation.y = self.robot.joint_ang[i * 2 + 1]
CMD.transforms.append(trans_i)
twist_i = Twist()
CMD.twist.append(twist_i)
wrench_i = Wrench()
CMD.wrench.append(wrench_i)
self.pub_joint_cmd.publish(CMD)
if data.buttons[2] == 1:
print("Reset Simulation")
self.reset_sim()
def recievedirection(self):
birdinput = self.commport.read(1)
if birdinput == 'w':
wrench = Wrench(force=Vector3(2, 0, 0), torque=Vector3(0, 0, 0))
elif birdinput == 's':
wrench = Wrench(force=Vector3(-2, 0, 0), torque=Vector3(0, 0, 0))
elif birdinput == 'a':
wrench = Wrench(force=Vector3(0, 0, 0), torque=Vector3(0, 0, 2))
elif birdinput == 'd':
wrench = Wrench(force=Vector3(0, 0, 0), torque=Vector3(0, 0, -2))
elif birdinput == 'x':
wrench = Wrench(force=Vector3(0, 0, 0), torque=Vector3(0, 0, 0))
self.wrench_pub.publish(
WrenchStamped(
header=Header(frame_id='/base_link', ),
wrench=wrench,
))
def ramped_wrench(prev, target, t_prev, t_now, ramps):
wr = Wrench()
wr.force.x = ramped_force(prev.force.x, target.force.x, t_prev, t_now,
ramps[0])
wr.force.y = ramped_force(prev.force.y, target.force.y, t_prev, t_now,
ramps[1])
wr.torque.z = ramped_force(prev.torque.z, target.torque.z, t_prev, t_now,
ramps[2])
return wr
def p(fx, fy, fz, tx, ty, tz): m = Wrench() m.force.x = fx m.force.y = fy m.force.z = fz m.torque.x = tx m.torque.y = ty m.torque.z = tz pub.publish(m)
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 initVariables(self):
self.vPrima = self.vLast = self.vCurrent = 0
self.wPrima = self.wLast = self.wCurrent = 0
self.dt = self.lastTime = 0
self.msgForce = Wrench()
self.msgVel = Twist()
self.changeWrench = self.changeTrq = self.changeFrc = False
self.rate = self.rospy.Rate(self.controlRate)
return
def array_to_wrench(array):
msg = Wrench()
msg.force.x = array[0]
msg.force.y = array[1]
msg.force.z = array[2]
msg.torque.x = array[3]
msg.torque.y = array[4]
msg.torque.z = array[5]
return msg
def dq_to_geometry_msgs_wrench(force, torque):
wrench = Wrench()
wrench.force.x = force.q[1]
wrench.force.y = force.q[2]
wrench.force.z = force.q[3]
wrench.torque.x = torque.q[1]
wrench.torque.y = torque.q[2]
wrench.torque.z = torque.q[3]
return wrench
def update(self, timestamp, pose, angular_velocity, linear_acceleration):
# Prepare state vector (pose only; ignore angular_velocity, linear_acceleration)
position = np.array([pose.position.x, pose.position.y, pose.position.z])
orientation = np.array([pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w])
# Calculate velocity
if self.last_timestamp is None:
velocity = np.array([0.0, 0.0, 0.0])
else:
velocity = (position - self.last_position) / max(timestamp - self.last_timestamp, 1e-03) # prevent divide by zero
# Create state space and update lag variables
state = np.concatenate([position, orientation, velocity])
self.last_timestamp = timestamp
self.last_position = position
# Compute reward / penalty and check if this episode is complete
done = False
error_position = abs(self.target_z - pose.position.z)
error_velocity = (self.target_velocity_z - velocity[2])**2
# reward = zero for matching target z, -ve as you go farther, upto -20
reward = -min(self.weight_position * error_position + self.weight_velocity * error_velocity, 20.0)
if pose.position.z >= self.target_z:
reward += 10.0
done = True
elif timestamp > self.max_duration:
reward -= 10.0
done = True
# Take one RL step, passing in current state and reward, and obtain action
# Note: The reward passed in here is the result of past action(s)
action = self.agent.step(state, reward, done) # note: action = <force; torque> vector
# Convert to proper force command (a Wrench object) and return it
if action is not None:
action = np.clip(action.flatten(), self.action_space.low, self.action_space.high) # flatten, clamp to action space limits
return Wrench(
force=Vector3(action[0], action[1], action[2]),
torque=Vector3(action[3], action[4], action[5])
), done
else:
return Wrench(), done
def set_wrench_body_force(self, force):
"Apply a wrench with force only (body), torque is null"
w = Wrench()
w.force.x = force[0]
w.force.y = force[1]
w.force.z = force[2]
w.torque.x = 0.0
w.torque.y = 0.0
w.torque.z = 0.0
self.__set_wrench_body_pub.publish(w)
def callbackHapkitPos(self, msg):
self.change_pos = True
self.pos = msg.data
self.msgWrench = Wrench()
if self.hapticMode == "free" or self.hapticMode == "visual" or self.hapticMode == "haptic" or self.hapticMode == "haptic visual":
self.msgWrench.torque.z = -1 * self.hapticParams["k1"] * np.tanh(
self.pos / self.hapticParams["k2"])
if self.msgWrench.torque.z < 0.5 and self.msgWrench.torque.z > -0.5:
self.msgWrench.torque.z = 0
pass
def _compute_control_wrench(self, frequency):
desired_state = State(7.0, 0.0)
measured_state = State(self._pose.position.z, self._twist.linear.z)
force_z = self._pid.output(desired_state, measured_state,
1.0 / frequency)
command_wrench = Wrench()
command_wrench.force.z = force_z
return command_wrench
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 update(self, timestamp, pose, angular_velocity, linear_acceleration):
self.count += 1
pos = np.array([pose.position.x, pose.position.y, pose.position.z])
distance = np.linalg.norm(self.target - pos)
rescaled_pos = self.state_rescale(pos)
rescaled_tar = self.state_rescale(self.target)
pos_target_vec = rescaled_tar - rescaled_pos
delta_pos = (pos - self.last_pos)
self.last_pos = pos
state = np.concatenate(
(rescaled_pos, delta_pos * 10., pos_target_vec * 5.0), axis=-1)
state = state[None, :]
accel = np.array([
linear_acceleration.x, linear_acceleration.y, linear_acceleration.z
])
sum_accel = np.linalg.norm(accel)
done = False
reward = (5. - distance) * 0.3 - sum_accel * 0.05
target_z = max((self.initial_target_z / self.landing_time) *
(self.landing_time - timestamp), 0.0)
self.target = np.array([0.0, 0.0, target_z])
print('re={:.3} pt={:.3} tar={:.3}'.format(reward, pos_target_vec[2],
self.target[2]),
end='\r')
if timestamp > self.max_duration:
done = True
self.action = self.agent.step(state, reward, done)
#print('action={:.3} reward={:.3}'.format(self.action[2],reward),end='\r')
action = self.action * self.action_space_range[2] * 0.5
if action is not None:
action = np.clip(action.flatten(), self.action_space.low,
self.action_space.high
) # flatten, clamp to action space limits
return Wrench(force=Vector3(action[0], action[1], action[2]),
torque=Vector3(action[3], action[4],
action[5])), done
else:
return Wrench(), done
def thrust_allocator_callback(self, msg):
if len(msg.data) != self.num_thrusters:
self.get_logger.warn(
"Controller output thruster count doesn't match simulation thruster count!"
)
else:
for i in range(1, self.num_thrusters + 1):
wrench_msg = Wrench()
wrench_msg.force.z = msg.data[i - 1]
self.thrust_pub_dict[i].publish(wrench_msg)
def callbackTheta(self, msg):
theta = msg.data
f1 = (1 + np.tanh(theta)) * self.k_virtual
f2 = (1 - np.tanh(theta)) * self.k_virtual
self.virtual_trq = (f2 - f1) * self.d_sensors / 2.0
msg = Wrench()
msg.torque.z = self.virtual_trq
self.pub_virtual_wrench.publish(msg)
self.change_theta = True
return
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)
