def __init__(self,
pose=[0, 0, 0, 0, 0, 0, 1],
description='Target',
server_name='target_marker',
frame_id='exotica/world_frame',
marker_shape=Marker.ARROW,
marker_size=[0.2, 0.05, 0.05],
controls=[]):
self.position = kdl.Frame()
self.position_exo = exo.KDLFrame()
self.server = InteractiveMarkerServer(server_name)
self.int_marker = InteractiveMarker()
self.int_marker.header.frame_id = frame_id
self.int_marker.name = server_name
self.int_marker.description = description
self.int_marker.pose = list_to_pose(pose)
self.position_exo = exo.KDLFrame(pose)
self.position = kdl.Frame(
kdl.Rotation.Quaternion(self.int_marker.pose.orientation.x,
self.int_marker.pose.orientation.y,
self.int_marker.pose.orientation.z,
self.int_marker.pose.orientation.w),
kdl.Vector(self.int_marker.pose.position.x,
self.int_marker.pose.position.y,
self.int_marker.pose.position.z))
# create a grey box marker
visual_marker = Marker()
visual_marker.type = marker_shape
visual_marker.scale.x = marker_size[0]
visual_marker.scale.y = marker_size[1]
visual_marker.scale.z = marker_size[2]
visual_marker.color.r = 0.0
visual_marker.color.g = 0.5
visual_marker.color.b = 0.5
visual_marker.color.a = 1.0
# create a non-interactive control which contains the box
visual_control = InteractiveMarkerControl()
visual_control.always_visible = True
visual_control.markers.append(visual_marker)
# add the control to the interactive marker
self.int_marker.controls.append(visual_control)
self.addControl([1, 0, 0], InteractiveMarkerControl.ROTATE_AXIS)
self.addControl([0, 1, 0], InteractiveMarkerControl.ROTATE_AXIS)
self.addControl([0, 0, 1], InteractiveMarkerControl.ROTATE_AXIS)
self.addControl([1, 0, 0], InteractiveMarkerControl.MOVE_AXIS)
self.addControl([0, 1, 0], InteractiveMarkerControl.MOVE_AXIS)
self.addControl([0, 0, 1], InteractiveMarkerControl.MOVE_AXIS)
for control in controls:
self.int_marker.controls.append(control)
self.server.insert(self.int_marker, self.process_feedback)
self.server.applyChanges()
def transform_base_traj(base_pose, tf_base):
'''takes input of base_pose which is an array [x,y,rot] and a KDLFrame.
returns [x,y,z,r,p,yaw]
'''
# frame = exo.KDLFrame(np.concatenate((base_pose[0:2],[0,0,0],base_pose[2]),axis = 0))
frame = exo.KDLFrame(np.concatenate((base_pose[0:2],[0,0,0],[base_pose[2]])))
rel_goal = tf_base * frame
return rel_goal.get_translation_and_rpy()
def addControl(self, direction, control_type):
control = InteractiveMarkerControl()
quat = exo.KDLFrame([0, 0, 0] + direction + [1]).get_quaternion()
control.orientation.x = quat[0]
control.orientation.y = quat[1]
control.orientation.z = quat[2]
control.orientation.w = quat[3]
control.interaction_mode = control_type
self.int_marker.controls.append(control)
def handleAttaching(i, x, scene):
# Update state
scene.Update(x)
if i == 0:
# Reset object pose
scene.attachObjectLocal('Item', '',
exo.KDLFrame([0.6, -0.3, 0.5, 0, 0, 0, 1]))
if i == 1:
# Attach to end effector
scene.attachObject('Item', 'lwr_arm_6_link')
if i == 2:
# Detach
scene.detachObject('Item')
def process_feedback(self, feedback):
self.position = kdl.Frame(
kdl.Rotation.Quaternion(feedback.pose.orientation.x,
feedback.pose.orientation.y,
feedback.pose.orientation.z,
feedback.pose.orientation.w),
kdl.Vector(feedback.pose.position.x, feedback.pose.position.y,
feedback.pose.position.z))
self.position_exo = exo.KDLFrame([
feedback.pose.position.x, feedback.pose.position.y,
feedback.pose.position.z, feedback.pose.orientation.x,
feedback.pose.orientation.y, feedback.pose.orientation.z,
feedback.pose.orientation.w
])
def send_trajectory(received_traj,grasp_times, dt):
# set up hsr python interface
robot = hsrb_interface.Robot()
whole_body = robot.get('whole_body')
hsrb_gripper = robot.get('gripper')
omni_base = robot.get('omni_base')
try:
open_gripper(hsrb_gripper)
whole_body.move_to_go()
except:
tts.say('Fail to initialize.')
rospy.logerr('fail to init')
sys.exit()
print('going to start_location')
# go to starting location.
listener = tf.TransformListener()
listener.waitForTransform('/map','/my_start_pos', rospy.Time(0),rospy.Duration(3.0))
(start_pos, start_quat) = listener.lookupTransform('/map','/my_start_pos', rospy.Time(0))
start_state = exo.KDLFrame(start_pos + start_quat)
x,y,z,roll,pitch,yaw = start_state.get_translation_and_rpy()
print(yaw)
# go to starting position
omni_base.go_abs(x,y,yaw,1000.0)
whole_body.move_to_go()
# exit()
'''takes input of trajectory from hsr_meeting_table_aico, [grasp_start, grasp_duration]
and time step of trajectory in seconds.
'''
#default dt = 0.1. set to >0.1 for testing / velocity limits exceeded
dt = 0.15
# dt = 0.10
print("received")
print(np.shape(received_traj))
# midpoint = grasp_times[1]/2
grasp_t = grasp_times[0]# + midpoint
# workaround
# cli_arm, cli_base, cli_grip_follow_traj, cli_grip_force = setup()
cli_arm, cli_base = setup()
# set current hsr base position to zero so I can run trajectories multiple times by resetting
# gazebo with gazebo_reset.
base_posestamped = rospy.wait_for_message('/global_pose', PoseStamped)
base_pose = base_posestamped.pose
x = base_pose.position.x
y = base_pose.position.y
z = base_pose.position.z
qx= base_pose.orientation.x
qy= base_pose.orientation.y
qz= base_pose.orientation.z
qw= base_pose.orientation.w
tf_base = exo.KDLFrame([x,y,z,qx,qy,qz,qw])
time_list = np.arange(0.0,dt*len(received_traj),dt)
# rearrange traj
arm_list = received_traj[:,3:8]
base_list = received_traj[:,0:3]
arm_list = np.c_[arm_list,time_list]
# move relative to hsrb_base_link instead of map
base_list = np.array([transform_base_traj(base_list[i], tf_base) for i in range(len(base_list))])
#index of x,y,yaw
col_index = [0,1,5]
base_list = base_list[:, col_index]
base_list = np.c_[base_list, time_list]
# #remove the zero-th time step because it has an undefined velocity
base_list = base_list[1::]
arm_list = arm_list[1::]
# get user input to run traj in gazebo
print("you can run ./log_vel.py now.")
print("press enter to continue")
raw_input()
print("loading arm goal")
arm.load_arm_goal(arm_list,cli_arm,dt)
print("loading base goal")
base.load_base_goal(base_list,cli_base,dt)
# gripper controller doesnt work. not listed in lise_controllers. See setup(). on around line 30
# thus jank stuff with rospy sleep to get the gripper to close at the right time
print("loading gripper goal")
# gripper.load_gripper_goal(gripper_list,cli_grip_follow_traj)
#open gripper
# workaround
# gripper.load_gripper_goal(np.array([[0.8,0.5]]),cli_grip_follow_traj)
open_gripper(hsrb_gripper)
print("sleeping")
rospy.sleep(grasp_t*dt/0.1)
print("closing")
# close_gripper(cli_grip_force)
close_gripper(hsrb_gripper)
while not rospy.is_shutdown():
rospy.spin()
print('All Done :)')
print('shutting down rospy')
exit()
#!/usr/bin/env python
import pyexotica as exo
solver = exo.Setup.loadSolver(
'{exotica_examples}/resources/configs/ik_solver_demo.xml')
scene = solver.getProblem().getScene()
# Update the scene before calling FK or Jacobian!
scene.Update([0] * 7)
print("FK for the end effector:")
print(scene.fk('lwr_arm_7_link'))
print("Jacobian for the end effector:")
print(scene.jacobian('lwr_arm_7_link'))
print("FK for a relative frame:")
scene.Update([0.5] * 7)
print(scene.fk('lwr_arm_7_link', 'lwr_arm_3_link'))
print("FK for a relative frame with custom offsets:")
print(
scene.fk('lwr_arm_7_link', exo.KDLFrame([0, 0, 0.2, 0, 0, 0, 1]),
'lwr_arm_3_link', exo.KDLFrame()))
# FK and Jacobian have the same overloads
listener = tf.TransformListener()
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
try:
# get tranforms of bottle and table and tag_1 (edge of table) relative to map
(table_pos,
table_quat) = listener.lookupTransform('/map', '/table',
rospy.Time(0))
(bottle_pos,
bottle_quat) = listener.lookupTransform('/map', '/hsr_soda',
rospy.Time(0))
(table_edge_pos, table_edge_quat) = listener.lookupTransform(
'/map', '/tag_1', rospy.Time(0))
table = exo.KDLFrame(table_pos + table_quat)
bottle = exo.KDLFrame(bottle_pos + bottle_quat)
tag_1 = exo.KDLFrame(table_edge_pos + table_edge_quat)
# TODO(2021-12-20): Since the AprilTag recognition is not stable in orientation with the
# simulator on Noetic, overwrite the transform to 0,0,0 rpy
# THIS IS A HACK and should not be used on hardware/fixed later.
table = exo.KDLFrame(table_pos + [pi / 2, 0, 0])
bottle = exo.KDLFrame(bottle_pos + [0, 0, 0])
tag_1 = exo.KDLFrame(table_edge_pos + [0, 0, 0])
# table relative to bottle. Can find how to rotate bottle tf to match table
# how to rotate bottle to get to table tf
bottle_table = bottle.inverse() * table
# rotate the bottle tf so that it matches the orientation of the table
def send_trajectory(received_traj,grasp_times, dt):
robot = hsrb_interface.Robot()
whole_body = robot.get('whole_body')
hsrb_gripper = robot.get('gripper')
omni_base = robot.get('omni_base')
try:
open_gripper(hsrb_gripper)
whole_body.move_to_go()
except:
tts.say('Fail to initialize.')
rospy.logerr('fail to init')
sys.exit()
# print('going to start_location')
# listener = tf.TransformListener()
# listener.waitForTransform('/map','/my_start_pos', rospy.Time(0),rospy.Duration(3.0))
# (start_pos, start_quat) = listener.lookupTransform('/map','/my_start_pos', rospy.Time(0))
# start_state = exo.KDLFrame(start_pos + start_quat)
# x,y,z,roll,pitch,yaw = start_state.get_translation_and_rpy()
# print(yaw)
# omni_base.go_abs(x,y,yaw,1000.0)
# whole_body.move_to_go()
whole_body.move_to_neutral()
# exit()
'''takes input of trajectory from hsr_meeting_table_aico, [grasp_start, grasp_duration]
and time step of trajectory in seconds.
'''
#default dt = 0.1. set to >0.1 for testing / velocity limits exceeded
dt = 0.2
# dt = 0.10
print("received")
print(np.shape(received_traj))
# midpoint = grasp_times[1]/2
grasp_t = grasp_times# + midpoint
# workaround
# cli_arm, cli_base, cli_grip_follow_traj, cli_grip_force = setup()
cli_arm, cli_base = setup()
base_posestamped = rospy.wait_for_message('/global_pose', PoseStamped)
base_pose = base_posestamped.pose
x = base_pose.position.x
y = base_pose.position.y
z = base_pose.position.z
qx= base_pose.orientation.x
qy= base_pose.orientation.y
qz= base_pose.orientation.z
qw= base_pose.orientation.w
tf_base = exo.KDLFrame([x,y,z,qx,qy,qz,qw])
time_list = np.arange(0.0,dt*len(received_traj),dt)
arm_list = received_traj[:,4:9]
base_list = received_traj[:,1:4]
arm_list = np.c_[arm_list,time_list]
base_list= base_list - base_list[0]
# base_list = base_list - base_list[0]
to_file(base_list, "base_list.txt")
# move relative to hsrb_base_link instead of map
# base_list = np.array([transform_base_traj(base_list[i], tf_base) for i in range(len(base_list))])
# to_file(base_list, "base_list_t.txt")
#index of x,y,yaw
col_index = [0,1,5]
# base_list = base_list[:, col_index]
base_list = np.c_[base_list, time_list]
# #remove the zero-th time step because it has an undefined velocity
base_list = base_list[1::]
arm_list = arm_list[1::]
# exit()
print("you can run ./log_vel.py now.")
print("press enter to continue")
input()
print("loading arm goal")
arm.load_arm_goal(arm_list,cli_arm,dt)
print("loading base goal")
base.load_base_goal(base_list,cli_base,dt)
print("loading gripper goal")
# gripper.load_gripper_goal(gripper_list,cli_grip_follow_traj)
#open gripper
# workaround
# gripper.load_gripper_goal(np.array([[0.8,0.5]]),cli_grip_follow_traj)
open_gripper(hsrb_gripper)
print("sleeping")
# gripper state = open
gripper_state = 0
# control gripper with this weirdness since action server for gripper doesnt work
if len(grasp_t) > 1:
gripper_action_times=np.diff(grasp_t)
for i in range(len(grasp_t)):
rospy.sleep(gripper_action_times*dt/0.1)
if gripper_state == 0:
close_gripper(hsrb_gripper)
else:
open_gripper(hsrb_gripper)
print("closing")
# close_gripper(cli_grip_force)
while not rospy.is_shutdown():
rospy.spin()
print('All Done :)')
print('shutting down rospy')
exit()
'{exotica_examples}/tests/resources/PrimitiveSphere_vs_PrimitiveSphere_Distance.urdf',
'{exotica_examples}/tests/resources/Mesh_vs_Mesh_Distance.urdf'
]
for urdf in urdfs_to_test:
print("Testing", urdf)
initializer = getProblemInitializer(collisionScene, urdf)
prob = exo.Setup.createProblem(initializer)
prob.update(np.zeros(prob.N, ))
scene = prob.getScene()
cs = scene.getCollisionScene()
# Should collide at -2
p = cs.continuousCollisionCheck("A_collision_0",
exo.KDLFrame([-3., 0.0, 0.0]),
exo.KDLFrame([-1.0, 0.0, 0.0]),
"B_collision_0", exo.KDLFrame([0, 0, 0]),
exo.KDLFrame([0, 0, 0]))
assert (p.InCollision == True)
assert ((p.TimeOfContact - 0.5) < 0.1)
assert (np.isclose(p.ContactTransform1.getTranslation(),
np.array([-2, 0, 0]),
atol=0.15).all())
assert (np.isclose(p.ContactTransform2.getTranslation(),
np.array([0, 0, 0])).all())
print(p)
print('>>SUCCESS<<')
exit(0)
def start_aico():
do_plot = True
traj_version = -1
use_screenshot = False
# ffmpeg -r 50 -f image2 -s 1920x1080 -i ./hsr_driveby_visualisation_%03d.png -vcodec libx264 -pix_fmt yuv420p ./output.mp4
screenshot = lambda *args: None
if use_screenshot:
from jsk_rviz_plugins.srv import Screenshot, ScreenshotRequest, ScreenshotResponse
rospy.wait_for_service('/rviz/screenshot')
screenshot = rospy.ServiceProxy('/rviz/screenshot', Screenshot)
exo.Setup.init_ros()
# config_name = '{hsr_driveby_full}/resources/hsr_meeting_room_table_aico_new.xml'
config_name = '{hsr_driveby_full}/resources/hsr_meeting_room_table_aico.xml'
solver = exo.Setup.load_solver(config_name)
problem = solver.get_problem()
scene = problem.get_scene()
kt = scene.get_kinematic_tree()
joint_limits = problem.get_scene().get_kinematic_tree().get_joint_limits()
# Set target for soda can
scene.attach_object("SodaCan", "TargetObject")
#scene.attach_object_local("TargetObject", "Table", exo.KDLFrame([0.2,0.30,0.06+0.04]))
#added offset to the y coordinate since planning and simulation grasp objects have different geometry.
# bottle on right hand side
# <!-- made changes here ** -->
scene.attach_object_local("TargetObject", "Table", exo.KDLFrame([0.2,0.30,0.06]))#+0.04]))
# bottle on left hand side
scene.attach_object_local("TargetObject", "Table", exo.KDLFrame([0.2,0.30,0.06]))#+0.04]))
# Move robot to start state
x_start = problem.start_state
x_start[0] = 0
x_start[1] = 0
x_start[2]= 0
# x_start[2] = -np.pi/2.
problem.start_state = x_start
scene.set_model_state(problem.start_state)
#scene.set_model_state_map({'hand_motor_joint': 0.7, 'hand_l_spring_proximal_joint':0.9, 'hand_l_distal_joint': -0.6, 'hand_r_spring_proximal_joint':0.9, 'hand_r_distal_joint': -0.6, 'wrist_roll_joint': -1.})
#set to move to go, assuming robot is already moving
# <!-- made changes here ** -->
# scene.set_model_state_map({'hand_motor_joint': 0.7, 'hand_l_spring_proximal_joint':0.9, 'hand_l_distal_joint': -0.6, 'hand_r_spring_proximal_joint':0.9, 'hand_r_distal_joint': -0.6, 'wrist_roll_joint': 0, 'wrist_flex_joint': -np.pi/2, 'arm_roll_joint': -np.pi/2})
scene.set_model_state_map({'hand_motor_joint': 0.7, 'hand_l_spring_proximal_joint':0.9, 'hand_l_distal_joint': -0.6, 'hand_r_spring_proximal_joint':0.9, 'hand_r_distal_joint': -0.6, 'wrist_roll_joint': 0, 'wrist_flex_joint': -np.pi/2, 'arm_roll_joint': 0})
problem.start_state = scene.get_model_state()
q_start = problem.apply_start_state(True)
q_start = np.clip(q_start, joint_limits[:,0], joint_limits[:,1])
problem.update(q_start, 0)
problem.start_state = scene.get_model_state()
q_start = problem.apply_start_state(True)
if np.any(q_start < joint_limits[:,0]) or np.any(q_start > joint_limits[:,1]):
raise RuntimeError("Start state exceeds joint limits!")
mug_location = scene.fk('SodaCan', exo.KDLFrame(), '', exo.KDLFrame()).get_translation_and_rpy()
# t_grasp_begin = 3.5 #4.2
t_grasp_begin = 4.5
t_grasp_duration = 0.5
T_grasp_begin = int(t_grasp_begin / problem.tau)
T_grasp_end = int((t_grasp_begin + t_grasp_duration) / problem.tau)
# The target position needs to be reached during the grasping period
problem.set_rho('Position', 0, 0)
for t in range(T_grasp_begin, T_grasp_end):
problem.set_rho('Position', 1e4, t)
problem.set_goal('Position', mug_location[:3], t)
# The HSR has a poor reachability, so we deactivate the base tracking here
# problem.set_rho('BasePosition', 0, t)
# Height above the table before and after grasp
problem.set_rho('LiftOffTable', 1e2, T_grasp_begin - 20)
# problem.set_rho('LiftOffTable', 1e3, T_grasp_end + 5)
# problem.set_rho('LiftOffTable', 1e4, T_grasp_end + 10)
problem.set_rho('LiftOffTable', 1e2, T_grasp_end + 20)
# The axis needs to be fixed from the beginning of the grasp to the end of the motion
for t in range(T_grasp_begin, problem.T):
#problem.set_rho('AxisAlignment', 1e4, t)
problem.set_rho('AxisAlignment', 1e2, t)
problem.set_rho('BaseOrientation', 1e2, -1)
# Initial trajectory = zero motion
zero_motion = np.zeros((problem.T,problem.N))
for t in range(problem.T):
zero_motion[t,:] = q_start
problem.initial_trajectory = zero_motion
solution = solver.solve()
print("Solved in", solver.get_planning_time(), "final cost", problem.get_cost_evolution()[1][-1])
# '''
# Show convergence plot
fig = plt.figure(1)
plt.plot(problem.get_cost_evolution()[0], problem.get_cost_evolution()[1])
plt.yscale('log')
plt.ylabel('Cost')
plt.xlabel('Time (s)')
plt.xlim(0,np.max(problem.get_cost_evolution()[0]))
plt.title('Convergence')
# Show cost breakdown
fig = plt.figure(2)
# '''
if do_plot:
costs = {}
ct = 1.0 / problem.tau / problem.T
for t in range(problem.T):
problem.update(solution[t,:],t)
for cost_task in problem.cost.indexing:
task = problem.cost.tasks[cost_task.id]
task_name = task.name
task_id = task.id
costs[task_name] = np.zeros((problem.T,))
# print(task_id, task_name, task, cost_task.start, cost_task.length, cost_task.startJ, cost_task.lengthJ)
for t in range(problem.T):
ydiff = problem.cost.ydiff[t][cost_task.startJ:cost_task.startJ+cost_task.lengthJ]
rho = problem.cost.S[t][cost_task.startJ:cost_task.startJ+cost_task.lengthJ,cost_task.startJ:cost_task.startJ+cost_task.lengthJ]
cost = np.dot(np.dot(ydiff, rho), ydiff)
costs[task_name][t] = ct * cost
# '''
if do_plot:
costs['Task'] = np.zeros((problem.T,))
costs['Transition'] = np.zeros((problem.T,))
for t in range(problem.T):
costs['Task'][t] = problem.get_scalar_task_cost(t)
costs['Transition'][t] = problem.get_scalar_transition_cost(t)
for cost in costs:
plt.plot(costs[cost], label=cost)
plt.legend()
plt.xlim(0,problem.T)
plt.title('Cost breakdown across trajectory per task')
plt.show()
plot(solution, labels=scene.get_controlled_joint_names())
print(mug_location)
return solution
publish_trajectory(solution, problem.T*problem.tau, problem)
