def get_hand_poses(self, tf_buffer, offset=geometry.pose()):
ref_to_hands = []
for object_to_hand in self._object_to_hands:
ref_to_hands.append(
geometry.multiply_tuples(self._ref_to_object, object_to_hand))
if self._approach_frame == _HAND_FRAME:
ref_to_hand_offsets = []
for ref_to_hand in ref_to_hands:
ref_to_hand_offsets.append(
geometry.multiply_tuples(ref_to_hand, offset))
return ref_to_hand_offsets
else:
hand_to_approach_tf = tf_buffer.lookup_transform(
_HAND_FRAME,
self._approach_frame,
# rospy.get_rostime(),
rospy.Time(0),
rospy.Duration(1.0))
hand_to_approach = geometry.transform_to_tuples(
hand_to_approach_tf.transform)
ref_to_hand_offsets = []
for ref_to_hand in ref_to_hands:
ref_to_approach = geometry.multiply_tuples(
ref_to_hand, hand_to_approach)
ref_to_approach_offset = geometry.multiply_tuples(
ref_to_approach, offset)
ref_to_hand_offset = geometry.multiply_tuples(
ref_to_approach_offset, _inverse_tuples(hand_to_approach))
ref_to_hand_offsets.append(ref_to_hand_offset)
return ref_to_hand_offsets
def _transform_base_trajectory(self, base_traj, odom_to_frame_pose):
odom_to_frame = geometry.pose_to_tuples(odom_to_frame_pose)
num_points = len(base_traj.points)
odom_base_traj = JointTrajectory()
odom_base_traj.points = list(
utils.iterate(JointTrajectoryPoint, num_points))
odom_base_traj.header = base_traj.header
odom_base_traj.joint_names = self._base_joint_names
# Transform each point into odom frame
previous_theta = 0.0
for i in range(num_points):
t = base_traj.points[i].transforms[0]
frame_to_base = geometry.transform_to_tuples(t)
# odom_to_base = odom_to_frame * frame_to_base
(odom_to_base_trans, odom_to_base_rot) = geometry.multiply_tuples(
odom_to_frame, frame_to_base)
odom_base_traj.points[i].positions = [
odom_to_base_trans[0], odom_to_base_trans[1], 0
]
roll, pitch, yaw = T.euler_from_quaternion(odom_to_base_rot)
dtheta = geometry.shortest_angular_distance(previous_theta, yaw)
theta = previous_theta + dtheta
odom_base_traj.points[i].positions[2] = theta
previous_theta = theta
return odom_base_traj
def manipulation_srv(self, action):
"""
Here the grasp precompute action (TU/e) is translated to a PlanWithHandGoals (TMC) and send as goal to the robot
:param action: the GraspPrecomputeAction type
"""
success = True
pose_quaternion = quaternion_from_euler(action.goal.roll,
action.goal.pitch,
action.goal.yaw)
static_quaternion = quaternion_from_euler(3.14159265359,
-1.57079632679, 0)
final_quaternion = quaternion_multiply(pose_quaternion,
static_quaternion)
pose = [action.goal.x, action.goal.y, action.goal.z], final_quaternion
################################################################################################################
# This piece of code is partially copied from Toyota software, it also uses the private functions (we're very
# sorry). Setting base_movement_type.val allows for adapting the allowed movements during manipulation.
ref_frame_id = settings.get_frame('base')
ref_to_hand_poses = [pose]
odom_to_ref_pose = self.whole_body._lookup_odom_to_ref(ref_frame_id)
odom_to_ref = geometry.pose_to_tuples(odom_to_ref_pose)
odom_to_hand_poses = []
for ref_to_hand in ref_to_hand_poses:
odom_to_hand = geometry.multiply_tuples(odom_to_ref, ref_to_hand)
odom_to_hand_poses.append(geometry.tuples_to_pose(odom_to_hand))
req = self.whole_body._generate_planning_request(
PlanWithHandGoalsRequest)
req.origin_to_hand_goals = odom_to_hand_poses
req.ref_frame_id = self.whole_body._end_effector_frame
req.base_movement_type.val = BaseMovementType.ROTATION_Z
service_name = self.whole_body._setting['plan_with_hand_goals_service']
plan_service = rospy.ServiceProxy(service_name, PlanWithHandGoals)
res = plan_service.call(req)
if res.error_code.val != ArmManipulationErrorCodes.SUCCESS:
rospy.logerr('Fail to plan move_endpoint')
success = False
else:
res.base_solution.header.frame_id = settings.get_frame('odom')
constrained_traj = self.whole_body._constrain_trajectories(
res.solution, res.base_solution)
self.whole_body._execute_trajectory(constrained_traj)
return success
def _transform_base_trajectory(self, base_traj):
"""Transform a base trajectory to an ``odom`` frame based trajectory.
Args:
base_traj (tmc_manipulation_msgs.msg.MultiDOFJointTrajectory):
A base trajectory
Returns:
trajectory_msgs.msg.JointTrajectory:
A base trajectory based on ``odom`` frame.
"""
odom_to_frame_transform = self._tf2_buffer.lookup_transform(
_BASE_TRAJECTORY_ORIGIN, base_traj.header.frame_id, rospy.Time(0),
rospy.Duration(_TF_TIMEOUT))
odom_to_frame = geometry.transform_to_tuples(
odom_to_frame_transform.transform)
num_points = len(base_traj.points)
odom_base_traj = JointTrajectory()
odom_base_traj.points = list(
utils.iterate(JointTrajectoryPoint, num_points))
odom_base_traj.header = base_traj.header
odom_base_traj.joint_names = self._base_client.joint_names
# Transform each point into odom frame
previous_theta = 0.0
for i in range(num_points):
t = base_traj.points[i].transforms[0]
frame_to_base = geometry.transform_to_tuples(t)
# odom_to_base = odom_to_frame * frame_to_base
(odom_to_base_trans, odom_to_base_rot) = geometry.multiply_tuples(
odom_to_frame, frame_to_base)
odom_base_traj.points[i].positions = [
odom_to_base_trans[0], odom_to_base_trans[1], 0
]
roll, pitch, yaw = T.euler_from_quaternion(odom_to_base_rot)
dtheta = geometry.shortest_angular_distance(previous_theta, yaw)
theta = previous_theta + dtheta
odom_base_traj.points[i].positions[2] = theta
previous_theta = theta
return odom_base_traj
def main(whole_body, gripper, wrist_wrench):
# Grab the handle of door
# wrist_wrench =wholjjjjjjjjjk
whole_body.move_to_neutral()
switch = ImpedanceControlSwitch()
wrist_wrench.reset()
gripper.command(1.0)
grab_pose = geometry.multiply_tuples(
geometry.pose(x=HANDLE_POS[0] - HANDLE_TO_HAND_POS,
y=HANDLE_POS[1],
z=HANDLE_POS[2],
ej=math.pi / 2), geometry.pose(ek=math.pi / 2))
whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF)
switch.activate("grasping")
# gripper.command(0.01)
gripper.grasp(-0.05)
rospy.sleep(1.0)
switch.inactivate()
rospy.sleep(10.0)
listener = tf.TransformListener()
listener.waitForTransform("/map", "/hand_palm_link", rospy.Time(),
rospy.Duration(4.0))
now = rospy.Time.now()
listener.waitForTransform("/map", "/hand_palm_link", now,
rospy.Duration(4.0))
# (trans, rot) = listener.lookupTransform("/map", "/hand_palm_link", now)
# cur_tuples = geometry.transform_to_tuples(target_trans)
# print trans
# print rot
#tf frame
# br = tf2_ros.TransformBroadcaster()
# t = geometry_msgs.msg.TransformStamped()
# t.header.stamp = rospy.Time.now()
# t.header.frame_id = "/map"
# t.child_frame_id = "/target"
# t.transform.translation.x = trans[0]
# t.transform.translation.y = trans[1]
# t.transform.translation.z = trans[2]
# q = tf.transformations.quaternion_from_euler(0, 0, 0.0)
# t.transform.rotation.x = rot[0]
# t.transform.rotation.y = rot[1]
# t.transform.rotation.z = rot[2]
# t.transform.rotation.w = rot[3]
# br.sendTransform(t)
# Rotate the handle (Angle: math.pi/6)
odom_to_hand = get_relative_tuples(_ORIGIN_TF, _HAND_TF)
tsr_to_odom = geometry.pose(
x=-(HANDLE_POS[0] + HANDLE_TO_HAND_POS),
y=-(HANDLE_POS[1] + HANDLE_TO_HANDLE_HINGE_POS),
z=-HANDLE_POS[2])
tsr_to_hand = geometry.multiply_tuples(tsr_to_odom, odom_to_hand)
const_tsr = TaskSpaceRegion()
const_tsr.end_frame_id = _HAND_TF
const_tsr.origin_to_tsr = geometry.tuples_to_pose(
geometry.pose(x=HANDLE_POS[0] + HANDLE_TO_HAND_POS,
y=HANDLE_POS[1] + HANDLE_TO_HANDLE_HINGE_POS,
z=HANDLE_POS[2]))
const_tsr.tsr_to_end = geometry.tuples_to_pose(tsr_to_hand)
const_tsr.min_bounds = [0, 0.0, 0.0, -(math.pi / 5), 0, 0]
const_tsr.max_bounds = [0, 0.0, 0.0, 0, 0, 0]
goal_tsr = TaskSpaceRegion()
goal_tsr.end_frame_id = _HAND_TF
goal_tsr.origin_to_tsr = geometry.tuples_to_pose(
geometry.pose(x=HANDLE_POS[0] + HANDLE_TO_HAND_POS,
y=HANDLE_POS[1] + HANDLE_TO_HANDLE_HINGE_POS,
z=HANDLE_POS[2]))
goal_tsr.tsr_to_end = geometry.tuples_to_pose(tsr_to_hand)
goal_tsr.min_bounds = [0, 0.0, 0.0, -math.pi / 5, 0, 0]
goal_tsr.max_bounds = [0, 0.0, 0.0, -math.pi / 5, 0, 0]
response = call_tsr_plan_service(whole_body, [const_tsr], [goal_tsr])
if response.error_code.val != ArmManipulationErrorCodes.SUCCESS:
rospy.logerr("Planning failed: (Error Code {0})".format(
response.error_code.val))
exit(-1)
response.base_solution.header.frame_id = _ORIGIN_TF
constrain_traj = whole_body._constrain_trajectories(
response.solution, response.base_solution)
whole_body._execute_trajectory(constrain_traj)
rospy.sleep(10.0)
# listener = tf.TransformListener()
now = rospy.Time.now()
listener.waitForTransform("/map", "/hand_palm_link", now,
rospy.Duration(4.0))
rospy.sleep(1.0)
def opendoor(req):
# main(whole_body, gripper,wrist_wrench)
frame = req.handle_pose.header.frame_id
hanlde_pos = req.handle_pose.pose
# hanlde_pos=geometry_msgs.msg.PoseStamped()
res = OpendoorResponse()
cli = actionlib.SimpleActionClient('/hsrb/omni_base_controller/follow_joint_trajectory', control_msgs.msg.FollowJointTrajectoryAction)
vel_pub = rospy.Publisher('/hsrb/command_velocity', geometry_msgs.msg.Twist, queue_size=10)
armPub = rospy.Publisher('/hsrb/arm_trajectory_controller/command', JointTrajectory, queue_size=1)
robot = hsrb_interface.Robot()
whole_body = robot.get('whole_body')
gripper = robot.get('gripper')
wrist_wrench = robot.get('wrist_wrench')
base=robot.get('omni_base')
start_theta=base.pose[2]
# with hsrb_interface.Robot() as robot:
# whole_body = robot.get('whole_body')
# gripper = robot.get('gripper')
# wrist_wrench = robot.get('wrist_wrench')
try:
# Open the gripper
whole_body.move_to_neutral()
grasp_point_client()
global recog_pos
global is_found
print recog_pos.pose.position
print("Opening the gripper")
whole_body.move_to_neutral()
rospy.sleep(2.5)
switch = ImpedanceControlSwitch()
gripper.command(1.0)
# Approach to the door
listener = tf.TransformListener()
listener.waitForTransform(_ORIGIN_TF,_BASE_TF, rospy.Time(), rospy.Duration(4.0))
# translation,rot = listener.lookupTransform(_BASE_TF, _ORIGIN_TF, rospy.Time())
recog_pos.header.frame_id=_ORIGIN_TF
recog_pos2 = listener.transformPose(_BASE_TF, recog_pos)
print("Approach to the door")
grab_pose = geometry.multiply_tuples(geometry.pose(x=recog_pos2.pose.position.x-HANDLE_TO_HAND_POS_X,
y=recog_pos2.pose.position.y,
z=recog_pos2.pose.position.z,
ej=math.pi/2),
geometry.pose(ek=math.pi/2))
# grab_pose = geometry.multiply_tuples(geometry.pose(x=recog_pos2.pose.position.x-HANDLE_TO_HAND_POS_X/2,
# y=recog_pos2.pose.position.y-HANDLE_TO_HAND_POS_X/2,
# z=recog_pos2.pose.position.z,
# ej=0,
# ek=math.pi/2), geometry.pose(ei=math.pi/2))
# grab_pose = geometry.multiply_tuples(geometry.pose(x=recog_pos.pose.position.x-translation[0]-HANDLE_TO_HAND_POS_X,
# y=recog_pos.pose.position.y-translation[1],
# z=recog_pos.pose.position.z-translation[2],
# ej=math.pi/2),
# geometry.pose(ek=0.0))
# # geometry.pose(ek=math.pi/2))
whole_body.move_end_effector_pose(grab_pose, _BASE_TF)
whole_body.move_end_effector_by_line((0, 0, 1), 0.06)
# Close the gripper
wrist_wrench.reset()
switch.activate("grasping")
gripper.grasp(-0.01)
rospy.sleep(1.0)
switch.inactivate()
# rospy.sleep(100)
# Rotate the handle
whole_body.impedance_config = 'grasping'
whole_body.move_end_effector_by_line((0,1,0),-0.015)
rospy.sleep(1)
whole_body.move_end_effector_by_line((0,0,1),-0.06)
rospy.sleep(1)
whole_body.move_end_effector_by_line((0,1,0),0.015)
# traj = JointTrajectory()
# # This controller requires that all joints have values
# traj.joint_names = ["arm_lift_joint", "arm_flex_joint",
# "arm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
# p = JointTrajectoryPoint()
# current_positions = [latest_positions[name] for name in traj.joint_names]
# current_positions[0] = latest_positions["arm_lift_joint"]-0.0675
# current_positions[1] = latest_positions["arm_flex_joint"]-0.02
# current_positions[2] = latest_positions["arm_roll_joint"]
# current_positions[3] = latest_positions["wrist_flex_joint"]
# current_positions[4] = latest_positions["wrist_roll_joint"]-0.65
# p.positions = current_positions
# p.velocities = [0, 0, 0, 0, 0]
# p.time_from_start = rospy.Time(3)
# traj.points = [p]
# armPub.publish(traj)
# rospy.sleep(3.0)
# print("finishing rotating handle")
# ## Move by End-effector line
# whole_body.impedance_config = 'compliance_hard'
# tw = geometry_msgs.msg.Twist()
# tw.linear.x = -0.3
# tw.angular.z = 0.45
# vel_pub.publish(tw)
# rospy.sleep(2.0)
# vel_pub.publish(tw)
# rospy.sleep(2.0)
# print("test2 ")
# tw.linear.x =-0.3
# tw.angular.z = -0.45
# vel_pub.publish(tw)
# rospy.sleep(2.0)
# vel_pub.publish(tw)
# rospy.sleep(2.0)
# gripper.command(1.0)
# rospy.sleep(1.0)
# tw.linear.x = -0.3
# tw.angular.z = 0.0
# vel_pub.publish(tw)
# rospy.sleep(1.0)
# tw = geometry_msgs.msg.Twist()
# tw.linear.x = -0.15
# tw.linear.y = -0.45
# vel_pub.publish(tw)
tw = geometry_msgs.msg.Twist()
tw.linear.x = -0.3
tw.angular.z = 0.45
vel_pub.publish(tw)
rospy.sleep(2.0)
vel_pub.publish(tw)
rospy.sleep(2.0)
print("test2 ")
tw.linear.x =-0.3
tw.angular.z = -0.45
vel_pub.publish(tw)
rospy.sleep(2.0)
vel_pub.publish(tw)
rospy.sleep(2.0)
gripper.command(1.0)
rospy.sleep(1.0)
tw.linear.x = -0.3
tw.angular.z = 0.0
vel_pub.publish(tw)
rospy.sleep(1.0)
tw = geometry_msgs.msg.Twist()
tw.linear.x = -0.15
tw.linear.y = -0.45
vel_pub.publish(tw)
def execute(self, goal):
print("Start action")
# main(whole_body, gripper,wrist_wrench)
# frame = goal.handle_pose.header.frame_id
# hanlde_pos = goal.handle_pose.pose
# hanlde_pos=geometry_msgs.msg.PoseStamped()
cli = actionlib.SimpleActionClient(
'/hsrb/omni_base_controller/follow_joint_trajectory',
control_msgs.msg.FollowJointTrajectoryAction)
vel_pub = rospy.Publisher('/hsrb/command_velocity',
geometry_msgs.msg.Twist,
queue_size=10)
armPub = rospy.Publisher('/hsrb/arm_trajectory_controller/command',
JointTrajectory,
queue_size=1)
robot = hsrb_interface.Robot()
whole_body = robot.get('whole_body')
gripper = robot.get('gripper')
wrist_wrench = robot.get('wrist_wrench')
base = robot.get('omni_base')
start_theta = base.pose[2]
# with hsrb_interface.Robot() as robot:
# whole_body = robot.get('whole_body')
# gripper = robot.get('gripper')
# wrist_wrench = robot.get('wrist_wrench')
try:
# Open the gripper
whole_body.move_to_neutral()
grasp_point_client()
global recog_pos
global is_found
print recog_pos.pose.position
print("Opening the gripper")
whole_body.move_to_neutral()
rospy.sleep(2.5)
switch = ImpedanceControlSwitch()
gripper.command(1.0)
# Approach to the door
print("Approach to the door")
grab_pose = geometry.multiply_tuples(
geometry.pose(
x=recog_pos.pose.position.x - HANDLE_TO_HAND_POS_X,
y=recog_pos.pose.position.y - HANDLE_TO_HAND_POS_Y,
z=recog_pos.pose.position.z,
ej=math.pi / 2), geometry.pose(ek=math.pi / 2))
whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF)
# Close the gripper
wrist_wrench.reset()
switch.activate("grasping")
gripper.grasp(-0.01)
rospy.sleep(1.0)
switch.inactivate()
# Rotate the handle
whole_body.impedance_config = 'grasping'
traj = JointTrajectory()
# This controller requires that all joints have values
traj.joint_names = [
"arm_lift_joint", "arm_flex_joint", "arm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
p = JointTrajectoryPoint()
current_positions = [
latest_positions[name] for name in traj.joint_names
]
current_positions[0] = latest_positions["arm_lift_joint"] - 0.0675
current_positions[1] = latest_positions["arm_flex_joint"] - 0.02
current_positions[2] = latest_positions["arm_roll_joint"]
current_positions[3] = latest_positions["wrist_flex_joint"]
current_positions[4] = latest_positions["wrist_roll_joint"] - 0.65
p.positions = current_positions
p.velocities = [0, 0, 0, 0, 0]
p.time_from_start = rospy.Time(3)
traj.points = [p]
armPub.publish(traj)
rospy.sleep(3.0)
print("finishing rotating handle")
## Move by End-effector line
whole_body.impedance_config = 'compliance_hard'
whole_body.move_end_effector_by_line((0.0, 0.0, 1), 0.45)
print("push the door")
## Move base with linear & Angular motion
tw = geometry_msgs.msg.Twist()
tw.linear.x = 0.9
tw.angular.z = 0.45
vel_pub.publish(tw)
rospy.sleep(4.0)
## Move base with linear & Angular motion second
tw_cmd0 = geometry_msgs.msg.Twist()
tw_cmd0.linear.x = 0.5
tw_cmd0.angular.z = 0.45
vel_pub.publish(tw_cmd0)
rospy.sleep(2.0)
tw_cmd1 = geometry_msgs.msg.Twist()
tw_cmd1.linear.x = 0.6
tw_cmd1.linear.y = 0.2
tw_cmd1.angular.z = 0.25
vel_pub.publish(tw_cmd1)
# rospy.sleep(4.0)
rospy.sleep(3.0)
tw_cmd2 = geometry_msgs.msg.Twist()
tw_cmd2.linear.x = 0.65
tw_cmd2.linear.y = 0.5
tw_cmd2.angular.z = 0.35
vel_pub.publish(tw_cmd2)
# rospy.sleep(4.0)
rospy.sleep(2.0)
## Open the gripper
gripper.command(1.0)
## Move back
base.go_rel(-1.3, 0.0, start_theta)
gripper.command(1.0)
whole_body.move_to_neutral()
self.server.set_succeeded(True)
except Exception as e:
rospy.logerr(e)
print "Failed to open door"
self.server.set_succeeded(False)
def go_to(req):
print("starting go_to")
# hand_service = rospy.ServiceProxy("/openpose/hand_positions",GetHandPositions)
n_call = 3
global left_pos_f
global right_pos_f
left_pos_f.x = 0
left_pos_f.y = 0
left_pos_f.z = 0
right_pos_f.x = 0
right_pos_f.y = 0
right_pos_f.z = 0
for i in range(0, n_call):
rospy.sleep(2)
rospy.wait_for_service("/openpose/hand_positions")
res1 = hand_service.call()
print("Received left and right :")
print(res1.left_hand)
print(res1.right_hand)
left_pos = res1.left_hand
right_pos = res1.right_hand
if (abs(left_pos.x + 0.059) > 0.04 and abs(left_pos.x + 0.059) > 0.04):
print(abs(left_pos.x + 0.059))
print(abs(right_pos.x + 0.059))
left_pos_f.x += left_pos.x
left_pos_f.y += left_pos.y
left_pos_f.z += left_pos.z
right_pos_f.x += right_pos.x
right_pos_f.y += right_pos.y
right_pos_f.z += right_pos.z
if left_pos.x < right_pos.x:
avoid = right_pos_f
good = left_pos_f
else:
avoid = left_pos_f
good = right_pos_f
good.x = good.x / n_call
good.y = good.y / n_call
good.z = good.z / n_call
avoid.x = good.x / n_call
avoid.y = good.y / n_call
avoid.z = good.z / n_call
vel_pub = rospy.Publisher('/hsrb/command_velocity',
geometry_msgs.msg.Twist,
queue_size=10)
print("accessing robot")
robot = hsrb_interface.Robot()
whole_body = robot.get('whole_body')
gripper = robot.get('gripper')
wrist_wrench = robot.get('wrist_wrench')
base = robot.get('omni_base')
print("accessing robot complete")
try:
# whole_body.move_to_neutral()
# print("looking for grasping point")
# grasp_point_client(avoid,good)
# global recog_pos
# global is_found
# print (recog_pos.pose.position)
print("Opening the gripper")
whole_body.move_to_neutral()
# rospy.sleep(2.5)
switch = ImpedanceControlSwitch()
gripper.command(1.0)
# Approach to the door
# listener = tf.TransformListener()
# listener.waitForTransform(_ORIGIN_TF,_BASE_TF, rospy.Time(), rospy.Duration(4.0))
# # translation,rot = listener.lookupTransform(_BASE_TF, _ORIGIN_TF, rospy.Time())
# recog_pos.header.frame_id=_ORIGIN_TF
# recog_pos2 = listener.transformPose(_BASE_TF, recog_pos)
recog_pos2.pose.position.x = good.x - 0.05
recog_pos2.pose.position.z = good.z - 0.07
recog_pos2.pose.position.y = good.y - 0.1
grab_pose = geometry.multiply_tuples(
geometry.pose(x=recog_pos2.pose.position.x - HANDLE_TO_HAND_POS_X,
y=recog_pos2.pose.position.y,
z=recog_pos2.pose.position.z,
ej=math.pi / 2,
ek=math.pi / 2), geometry.pose(ei=math.pi / 2))
whole_body.move_end_effector_pose(grab_pose, _BASE_TF)
whole_body.move_end_effector_by_line((0, 0, 1), 0.1)
# Close the gripper
wrist_wrench.reset()
switch.activate("grasping")
gripper.grasp(-0.01)
rospy.sleep(1.0)
switch.inactivate()
whole_body.impedance_config = 'grasping'
tw = geometry_msgs.msg.Twist()
tw.linear.x = -0.3
vel_pub.publish(tw)
rospy.sleep(2)
whole_body.move_to_neutral()
gripper.command(1.0)
except Exception as e:
rospy.logerr(e)
print("Failed to open door")
# res.success = False
res = True
return res
def main(whole_body, base, gripper, wrist_wrench):
cli = actionlib.SimpleActionClient(
'/hsrb/omni_base_controller/follow_joint_trajectory',
control_msgs.msg.FollowJointTrajectoryAction)
# publisher for delvering command for base move
vel_pub = rospy.Publisher('/hsrb/command_velocity',
geometry_msgs.msg.Twist,
queue_size=10)
# base_pub = rospy.Publisher('/move_base/move/goal',move_base_msgs.msg.MoveBaseActionGoal,queue_size=10)
armPub = rospy.Publisher('/hsrb/arm_trajectory_controller/command',
JointTrajectory,
queue_size=1)
## Grab the handle of door
#test with service - get the handle position from handle
grasp_point_client()
global recog_pos
global Is_found
print recog_pos.pose.position
# target_pose_Msg = rospy.wait_for_message("/handle_detector/grasp_point", PoseStamped)
# recog_pos.pose.position.x=target_pose_Msg.pose.position.x
# recog_pos.pose.position.y=target_pose_Msg.pose.position.y
# recog_pos.pose.position.z=target_pose_Msg.pose.position.z
whole_body.move_to_neutral()
# whole_body.impedance_config= 'grasping'
switch = ImpedanceControlSwitch()
# wrist_wrench.reset()
gripper.command(1.0)
grab_pose = geometry.multiply_tuples(
geometry.pose(x=recog_pos.pose.position.x - HANDLE_TO_HAND_POS_X,
y=recog_pos.pose.position.y - HANDLE_TO_HAND_POS_Y,
z=recog_pos.pose.position.z,
ej=math.pi / 2), geometry.pose(ek=math.pi / 2))
whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF)
wrist_wrench.reset()
# whole_body.impedance_config= 'compliance_middle'
switch.activate("grasping")
# gripper.command(0.01)
gripper.grasp(-0.01)
rospy.sleep(1.0)
switch.inactivate()
wrist_wrench.reset()
rospy.sleep(8.0)
#### test manipulation
#change the impedance config to grasping
whole_body.impedance_config = 'grasping'
## Direct Joint trajectory
traj = JointTrajectory()
# This controller requires that all joints have values
traj.joint_names = [
"arm_lift_joint", "arm_flex_joint", "arm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
p = JointTrajectoryPoint()
current_positions = [latest_positions[name] for name in traj.joint_names]
current_positions[0] = latest_positions["arm_lift_joint"] - 0.07
current_positions[1] = latest_positions["arm_flex_joint"] - 0.02
current_positions[2] = latest_positions["arm_roll_joint"]
current_positions[3] = latest_positions["wrist_flex_joint"]
current_positions[4] = latest_positions["wrist_roll_joint"] - 0.65
p.positions = current_positions
p.velocities = [0, 0, 0, 0, 0]
p.time_from_start = rospy.Time(3)
traj.points = [p]
armPub.publish(traj)
rospy.sleep(3.0)
## Move by End-effector line
whole_body.impedance_config = 'compliance_hard'
whole_body.move_end_effector_by_line((0.0, 0.0, 1), 0.45)
## Move base with linear & Angular motion
tw = geometry_msgs.msg.Twist()
tw.linear.x = 0.9
tw.angular.z = 0.45
vel_pub.publish(tw)
rospy.sleep(4.0)
## Move base with linear & Angular motion second
tw_cmd0 = geometry_msgs.msg.Twist()
tw_cmd0.linear.x = 0.3
tw_cmd0.angular.z = 0.45
vel_pub.publish(tw_cmd0)
# rospy.sleep(4.0)
rospy.sleep(4.0)
## Open the gripper
gripper.command(1.0)
## Move back
tw_cmd = geometry_msgs.msg.Twist()
tw_cmd.linear.x = -1.2
vel_pub.publish(tw_cmd)
rospy.sleep(2.0)
## Move back 2
tw_cmd2 = geometry_msgs.msg.Twist()
tw_cmd2.linear.x = -1.1
tw_cmd2.angular.z = -0.4
vel_pub.publish(tw_cmd2)
rospy.sleep(4.0)
whole_body.move_to_neutral()
## Move back
tw_cmd2 = geometry_msgs.msg.Twist()
tw_cmd2.linear.x = -0.6
tw_cmd2.angular.z = -0.3
vel_pub.publish(tw_cmd2)
navi_service_client(2.0, 0.0, 0.0)
def main(whole_body, gripper, wrist_wrench):
armPub = rospy.Publisher('/hsrb/arm_trajectory_controller/command',
JointTrajectory,
queue_size=1)
## Grab the handle of door
target_pose_Msg = rospy.wait_for_message("/handle_detector/grasp_point",
PoseStamped)
recog_pos.pose.position.x = target_pose_Msg.pose.position.x
recog_pos.pose.position.y = target_pose_Msg.pose.position.y
recog_pos.pose.position.z = target_pose_Msg.pose.position.z
whole_body.move_to_neutral()
# whole_body.impedance_config= 'grasping'
switch = ImpedanceControlSwitch()
# wrist_wrench.reset()
gripper.command(1.0)
grab_pose = geometry.multiply_tuples(
geometry.pose(x=recog_pos.pose.position.x - HANDLE_TO_HAND_POS,
y=recog_pos.pose.position.y,
z=recog_pos.pose.position.z,
ej=math.pi / 2), geometry.pose(ek=math.pi / 2))
whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF)
wrist_wrench.reset()
# whole_body.impedance_config= 'compliance_middle'
switch.activate("grasping")
# gripper.command(0.01)
gripper.grasp(-0.008)
rospy.sleep(1.0)
switch.inactivate()
wrist_wrench.reset()
rospy.sleep(8.0)
#### test manipulation
whole_body.impedance_config = 'grasping'
# print(whole_body.impedance_config)
# desired_rot=-1.95
# whole_body.move_to_joint_positions({"wrist_roll_joint":desired_rot})
wrist_roll = latest_positions["wrist_roll_joint"] - 0.55
traj = JointTrajectory()
# This controller requires that all joints have values
traj.joint_names = [
"arm_lift_joint", "arm_flex_joint", "arm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
p = JointTrajectoryPoint()
current_positions = [latest_positions[name] for name in traj.joint_names]
current_positions[0] = latest_positions["arm_lift_joint"] - 0.04
current_positions[1] = latest_positions["arm_flex_joint"] - 0.015
current_positions[2] = latest_positions["arm_roll_joint"]
current_positions[3] = latest_positions["wrist_flex_joint"]
current_positions[4] = wrist_roll
p.positions = current_positions
p.velocities = [0, 0, 0, 0, 0]
p.time_from_start = rospy.Time(3)
traj.points = [p]
armPub.publish(traj)
rospy.sleep(5.0)
# whole_body.end_effector_frame = u'odom'
# whole_body.move_end_effector_by_line((0, 0, 1), -0.2)
# publish_arm(latest_positions["arm_lift_joint"],latest_positions["arm_flex_joint"],latest_positions["arm_roll_joint"], latest_positions["wrist_flex_joint"],wrist_roll)
# whole_body.end_effector_frame = u'base_link'
whole_body.impedance_config = 'grasping'
whole_body.move_end_effector_by_line((0, 0, 1), 0.35)
whole_body.impedance_config = None
##
# whole_body.move_to_joint_positions({"wrist_roll_joint": -0.3})
# rospy.sleep(2.0)
# whole_body.end_effector_frame = u'odom'
# whole_body.move_end_effector_by_line((0, 0, 1), -0.2)
# whole_body.move_end_effector_by_arc(geometry.pose(x=0.2, y=0.25, z=0.38, ej=math.radians(90.0)), math.radians(60.0))
# whole_body.move_end_effector_by_arc(geometry.pose(y=0.45, z=0.08, ej=math.radians(90.0)), math.radians(60.0), ref_frame_id='hand_palm_link')
# listener = tf.TransformListener()
# listener.waitForTransform("/odom", "/hand_palm_link", rospy.Time().now(), rospy.Duration(3.0))
# now = rospy.Time.now()
# listener.waitForTransform("/odom", "/hand_palm_link", now, rospy.Duration(5.0))
# (trans, rot) = listener.lookupTransform("/map", "/hand_palm_link", now)
# cur_tuples = geometry.transform_to_tuples(target_trans)
# print trans
# print rot
# Rotate the handle (Angle: math.pi/6)
# wrist_wrench.reset()
# whole_body.end_effector_frame = 'hand_palm_link'
# whole_body.impedance_config= 'dumper_soft'
#############################
# odom_to_hand = get_relative_tuples(_ORIGIN_TF, _HAND_TF)
# tsr_to_odom = geometry.pose(x=-(recog_pos.pose.position.x+HANDLE_TO_HAND_POS),
# y=-(recog_pos.pose.position.y+HANDLE_TO_HANDLE_HINGE_POS),
# z=-recog_pos.pose.position.z)
# tsr_to_hand = geometry.multiply_tuples(tsr_to_odom, odom_to_hand)
# const_tsr = TaskSpaceRegion()
# const_tsr.end_frame_id = _HAND_TF
# const_tsr.origin_to_tsr = geometry.tuples_to_pose(geometry.pose(x=recog_pos.pose.position.x+HANDLE_TO_HAND_POS,
# y=recog_pos.pose.position.y+HANDLE_TO_HANDLE_HINGE_POS,
# z=recog_pos.pose.position.z))
# const_tsr.tsr_to_end = geometry.tuples_to_pose(tsr_to_hand)
# const_tsr.min_bounds = [0, 0.0, 0.0,-(math.pi/7) , 0, 0]
# const_tsr.max_bounds = [0, 0.0, 0.0, 0, 0, 0]
# goal_tsr = TaskSpaceRegion()
# goal_tsr.end_frame_id = _HAND_TF
# goal_tsr.origin_to_tsr = geometry.tuples_to_pose(geometry.pose(x=recog_pos.pose.position.x+HANDLE_TO_HAND_POS,
# y=recog_pos.pose.position.y+HANDLE_TO_HANDLE_HINGE_POS,
# z=recog_pos.pose.position.z))
# goal_tsr.tsr_to_end = geometry.tuples_to_pose(tsr_to_hand)
# goal_tsr.min_bounds = [0, 0.0, 0.0,-math.pi/7, 0, 0]
# goal_tsr.max_bounds = [0, 0.0, 0.0,-math.pi/7, 0, 0]
# response = call_tsr_plan_service(whole_body, [const_tsr], [goal_tsr])
# if response.error_code.val != ArmManipulationErrorCodes.SUCCESS:
# rospy.logerr("Planning failed: (Error Code {0})".format(response.error_code.val))
# exit(-1)
# response.base_solution.header.frame_id = _ORIGIN_TF
# constrain_traj = whole_body._constrain_trajectories(response.solution,
# response.base_solution)
# # wrist_wrench.reset()
# # switch.activate("grasping")
# whole_body._execute_trajectory(constrain_traj)
# # whole_body.impedance_config= 'dumper_soft'
# # switch.inactivate()
# rospy.sleep(10.0)
# listener = tf.TransformListener()
# now = rospy.Time.now()
# listener.waitForTransform("/odom", "/hand_palm_link", now, rospy.Duration(3.0))
# rospy.sleep(3.0)
# # Open the door (Angle: math.pi/4)
# #rist_wrench.reset()
# # switch.activate("placing")
# odom_to_hand = get_relative_tuples(_ORIGIN_TF, _HAND_TF) #T0h
# tsr_to_odom = geometry.pose(x=-(recog_pos.pose.position.x),
# y=-(recog_pos.pose.position.y+HANDLE_TO_DOOR_HINGE_POS+HANDLE_GOAL_OFFSET),
# z=-recog_pos.pose.position.z) #Twe
# tsr_to_hand = geometry.multiply_tuples(tsr_to_odom, odom_to_hand) #T0s'
# const_tsr = TaskSpaceRegion()
# const_tsr.end_frame_id = _HAND_TF
# const_tsr.origin_to_tsr = geometry.tuples_to_pose(geometry.pose(x=recog_pos.pose.position.x,
# y=recog_pos.pose.position.y+HANDLE_TO_DOOR_HINGE_POS+HANDLE_GOAL_OFFSET,
# z=recog_pos.pose.position.z))
# const_tsr.tsr_to_end = geometry.tuples_to_pose(tsr_to_hand)
# const_tsr.min_bounds = [0, 0.0, 0.0, 0, 0, 0]
# const_tsr.max_bounds = [0, 0.0, 0.0, 0, 0, math.pi/4]
# goal_tsr = TaskSpaceRegion()
# goal_tsr.end_frame_id = _HAND_TF
# goal_tsr.origin_to_tsr = geometry.tuples_to_pose(geometry.pose(x=recog_pos.pose.position.x,
# y=recog_pos.pose.position.y+HANDLE_TO_DOOR_HINGE_POS+HANDLE_GOAL_OFFSET,
# z=recog_pos.pose.position.z))
# goal_tsr.tsr_to_end = geometry.tuples_to_pose(tsr_to_hand)
# goal_tsr.min_bounds = [0, 0.0, 0.0, 0, 0, math.pi/4]
# goal_tsr.max_bounds = [0, 0.0, 0.0, 0, 0, math.pi/4]
# response = call_tsr_plan_service(whole_body, [const_tsr], [goal_tsr])
# if response.error_code.val != ArmManipulationErrorCodes.SUCCESS:
# rospy.logerr("Planning failed: (Error Code {0})".format(response.error_code.val))
# exit(-1)
# response.base_solution.header.frame_id = _ORIGIN_TF
# constrain_traj = whole_body._constrain_trajectories(response.solution,
# response.base_solution)
# whole_body._execute_trajectory(constrain_traj)
# # whole_body.impedance_config= None
# # switch.inactivate()
gripper.command(1.0)
whole_body.move_to_neutral()
def execute_cb(self, goal):
rospy.loginfo("hand detection for handover started")
n_call = 3
n_real_call = 0
n_alt_call = 0
good = geometry_msgs.msg.Point()
avoid = geometry_msgs.msg.Point()
try:
for i in range(0,n_call):
rospy.loginfo("running " + str(i) + " out of " + str(n_call) + " detections")
rospy.wait_for_service(GRAB_BAG_SRV_NAME)
res = self.grabbag_client()
if (abs(res.left_hand.x+0.059)>0.03 and abs(res.right_hand.x+0.059)>0.03):
if res.left_hand.x < res.right_hand.x:
avoid = res.right_hand
good = res.left_hand
else:
avoid = res.left_hand
good = res.right_hand
goodt.x += good.x
goodt.y += good.y
goodt.z += good.z
avoidt.x += avoid.x
avoidt.y += avoid.y
avoidt.z += avoid.z
n_real_call += 1
else :
if (res.left_hand.x > 0 and res.right_hand.x < 0):
goodalt = res.left_hand
avoidalt = res.right_hand
else :
goodalt = res.right_hand
avoidalt = res.left_hand
goodaltt.x += goodalt.x
goodaltt.y += goodalt.y
goodaltt.z += goodalt.z
n_alt_call += 1
if n_real_call != 0 :
good.x = goodt.x/n_real_call
good.y = goodt.y/n_real_call
good.z = goodt.z/n_real_call
avoid.x = avoidt.x/n_real_call
avoid.y = avoidt.y/n_real_call
avoid.z = avoidt.z/n_real_call
else :
print ("using goodalt")
good.x = goodaltt.x/n_alt_call
good.y = goodaltt.y/n_alt_call
good.z = goodaltt.z/n_alt_call
avoid.x = avoidaltt.x/n_alt_call
avoid.y = avoidaltt.y/n_alt_call
avoid.z = avoidaltt.z/n_alt_call
print(good)
except rospy.ServiceException as e:
rospy.logerr(e)
exit(1)
rospy.loginfo("hands detection done. proceeding to grasp object")
# self.body.move_to_joint_positions({"arm_lift_joint": 0.4, "arm_flex_joint": -0.9,"wrist_roll_joint":-1.2, "wrist_flex_joint":0.2})
self.open_gripper(1.0)
recog_pos.pose.position.x = good.x - 0.2
recog_pos.pose.position.z = good.z - 0.07
recog_pos.pose.position.y = good.y - 0.08
grab_pose = geometry.multiply_tuples(geometry.pose(x=recog_pos.pose.position.x,
y=recog_pos.pose.position.y,
z=recog_pos.pose.position.z,
ej=math.pi/2,
ek=math.pi/2), geometry.pose(ei=math.pi/2))
self.body.move_end_effector_pose(grab_pose, "base_link")
self.body.move_end_effector_by_line((0, 0, 1), 0.1)
self.close_gripper()
rospy.loginfo("handover action finished")
self._as.set_succeeded()
def main(whole_body, gripper):
# Initialize
inv_perspective_transform_client = rospy.ServiceProxy(_INV_PERSPECTIVE_TRANSFORM_SRV_NAME,
InversePerspectiveTransform)
grasp_planner_client = rospy.ServiceProxy(_GRASP_PLANNER_SRV_NAME,
GetGraspPattern)
try:
inv_perspective_transform_client.wait_for_service(timeout=_CONNECTION_TIMEOUT)
grasp_planner_client.wait_for_service(timeout=_CONNECTION_TIMEOUT)
except Exception as e:
rospy.logerr(e)
sys.exit(1)
# Gaze at the target object
gripper.command(1.0)
whole_body.move_to_joint_positions(_VIEW_POSITIONS)
rospy.sleep(1.0)
# Get the position of the target object from a camera image
inv_perspective_transform_req = InversePerspectiveTransformRequest()
target_point = Point2DStamped()
target_point.point.x = _TARGET_POINT_X
target_point.point.y = _TARGET_POINT_Y
inv_perspective_transform_req.points_2D.append(target_point)
inv_perspective_transform_req.depth_registration = True
inv_perspective_transform_req.target_frame = 'base_footprint'
try:
res = inv_perspective_transform_client(inv_perspective_transform_req)
except rospy.ServiceException as e:
rospy.logerr(e)
exit(1)
if len(res.points_3D) < 1:
rospy.logerr('There is no detected point')
exit(1)
if res.points_3D[0].point.z < 0.01:
rospy.logerr('The detected point is ground')
exit(1)
# Get the grasp patterns
grasp_planner_req = GetGraspPatternRequest()
grasp_planner_req.points.append(res.points_3D[0])
grasp_planner_req.search_pattern = GetGraspPatternRequest.kAbovePlane
try:
res = grasp_planner_client(grasp_planner_req)
except rospy.ServiceException as e:
rospy.logerr(e)
exit(1)
if len(res.grasp_patterns) < 1:
rospy.logerr('There is no grasp pattern')
exit(1)
# Grasp the object
target_hand_pose = geometry.multiply_tuples(geometry.pose_to_tuples(res.object_pose.pose),
geometry.pose_to_tuples(res.grasp_patterns[0].hand_frame))
target_hand_euler = tf.transformations.euler_from_quaternion([
target_hand_pose.ori.x,
target_hand_pose.ori.y,
target_hand_pose.ori.z,
target_hand_pose.ori.w])
target_pose = hsrb_interface.geometry.pose(x=target_hand_pose.pos.x,
y=target_hand_pose.pos.y,
z=target_hand_pose.pos.z,
ei=target_hand_euler[0],
ej=target_hand_euler[1],
ek=target_hand_euler[2])
whole_body.move_end_effector_pose(target_pose)
gripper.grasp(-0.1)
# Move to neutral position
whole_body.move_to_neutral()
def opendoor(req):
# main(whole_body, gripper,wrist_wrench)
frame = req.handle_pose.header.frame_id
hanlde_pos = req.handle_pose.pose
# hanlde_pos=geometry_msgs.msg.PoseStamped()
res = OpendoorResponse()
cli = actionlib.SimpleActionClient(
'/hsrb/omni_base_controller/follow_joint_trajectory',
control_msgs.msg.FollowJointTrajectoryAction)
vel_pub = rospy.Publisher('/hsrb/command_velocity',
geometry_msgs.msg.Twist,
queue_size=10)
armPub = rospy.Publisher('/hsrb/arm_trajectory_controller/command',
JointTrajectory,
queue_size=1)
robot = hsrb_interface.Robot()
whole_body = robot.get('whole_body')
gripper = robot.get('gripper')
wrist_wrench = robot.get('wrist_wrench')
base = robot.get('omni_base')
start_theta = base.pose[2]
# with hsrb_interface.Robot() as robot:
# whole_body = robot.get('whole_body')
# gripper = robot.get('gripper')
# wrist_wrench = robot.get('wrist_wrench')
try:
# Open the gripper
whole_body.move_to_neutral()
grasp_point_client()
global recog_pos
global is_found
print recog_pos.pose.position
print("Opening the gripper")
whole_body.move_to_neutral()
rospy.sleep(2.5)
switch = ImpedanceControlSwitch()
gripper.command(1.0)
# Approach to the door
listener = tf.TransformListener()
listener.waitForTransform(_ORIGIN_TF, _BASE_TF, rospy.Time(),
rospy.Duration(4.0))
# translation,rot = listener.lookupTransform(_BASE_TF, _ORIGIN_TF, rospy.Time())
recog_pos.header.frame_id = _ORIGIN_TF
recog_pos2 = listener.transformPose(_BASE_TF, recog_pos)
print("Approach to the door")
grab_pose = geometry.multiply_tuples(
geometry.pose(
x=recog_pos2.pose.position.x - HANDLE_TO_HAND_POS_X / 2,
y=recog_pos2.pose.position.y - HANDLE_TO_HAND_POS_X / 2,
z=recog_pos2.pose.position.z,
ej=math.pi / 2), geometry.pose(ek=math.pi / 2))
# grab_pose = geometry.multiply_tuples(geometry.pose(x=recog_pos2.pose.position.x-HANDLE_TO_HAND_POS_X/2,
# y=recog_pos2.pose.position.y-HANDLE_TO_HAND_POS_X/2,
# z=recog_pos2.pose.position.z,
# ej=0,
# ek=math.pi/2), geometry.pose(ei=math.pi/2))
# grab_pose = geometry.multiply_tuples(geometry.pose(x=recog_pos.pose.position.x-translation[0]-HANDLE_TO_HAND_POS_X,
# y=recog_pos.pose.position.y-translation[1],
# z=recog_pos.pose.position.z-translation[2],
# ej=math.pi/2),
# geometry.pose(ek=0.0))
# # geometry.pose(ek=math.pi/2))
whole_body.move_end_effector_pose(grab_pose, _BASE_TF)
whole_body.move_end_effector_by_line((1, 0, 1), 0.06)
# Close the gripper
wrist_wrench.reset()
switch.activate("grasping")
gripper.grasp(-0.01)
rospy.sleep(1.0)
switch.inactivate()
# rospy.sleep(100)
# Rotate the handle
whole_body.impedance_config = 'grasping'
whole_body.move_end_effector_by_line((0, 1, 0), -0.015)
# traj = JointTrajectory()
# # This controller requires that all joints have values
# traj.joint_names = ["arm_lift_joint", "arm_flex_joint",
# "arm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
# p = JointTrajectoryPoint()
# current_positions = [latest_positions[name] for name in traj.joint_names]
# current_positions[0] = latest_positions["arm_lift_joint"]-0.0675
# current_positions[1] = latest_positions["arm_flex_joint"]-0.02
# current_positions[2] = latest_positions["arm_roll_joint"]
# current_positions[3] = latest_positions["wrist_flex_joint"]
# current_positions[4] = latest_positions["wrist_roll_joint"]-0.65
# p.positions = current_positions
# p.velocities = [0, 0, 0, 0, 0]
# p.time_from_start = rospy.Time(3)
# traj.points = [p]
# armPub.publish(traj)
# rospy.sleep(3.0)
# print("finishing rotating handle")
# ## Move by End-effector line
# whole_body.impedance_config = 'compliance_hard'
# whole_body.move_end_effector_by_line((0.0,0.0,1), 0.45)
print("pull the door")
# Move base with linear & Angular motion
# hand_pos.header.frame_id=u'hand_palm_link'
print("test 1")
tw = geometry_msgs.msg.Twist()
tw.linear.x = -0.3
tw.angular.z = -0.45
vel_pub.publish(tw)
rospy.sleep(4.0)
print("test2 ")
tw = geometry_msgs.msg.Twist()
tw.linear.x = -1
tw.angular.z = -0.45
vel_pub.publish(tw)
rospy.sleep(4.0)
print("test 3 ")
tw = geometry_msgs.msg.Twist()
tw.linear.x = -1
tw.angular.z = -0.45
vel_pub.publish(tw)
#############keep
rospy.sleep(4.0)
print("test 4 ")
tw = geometry_msgs.msg.Twist()
tw.linear.x = -1
tw.angular.z = -0.45
vel_pub.publish(tw)
rospy.sleep(4.0)
print("test 5")
tw = geometry_msgs.msg.Twist()
tw.linear.x = -1
tw.angular.z = -0.45
vel_pub.publish(tw)
rospy.sleep(4.0)
print("test 6")
tw = geometry_msgs.msg.Twist()
tw.linear.x = -1
tw.angular.z = -0.45
vel_pub.publish(tw)
rospy.sleep(4.0)
print("test 7")
tw = geometry_msgs.msg.Twist()
tw.linear.x = -1
tw.angular.z = -0.45
vel_pub.publish(tw)
rospy.sleep(4.0)
##############################keep
## Move base orthogonally to the force
# force_sensor_capture = ForceSensorCapture()
# force_list = force_sensor_capture.get_current_force()
# a=-force_list[0]/10
# b=force_list[1]/10
# tw_cmd0 = geometry_msgs.msg.Twist()
# tw_cmd0.linear.x =-a
# tw_cmd0.linear.y = b
# vel_pub.publish(tw_cmd0)
# rospy.sleep(2.0)
# force_list = force_sensor_capture.get_current_force()
# a=-force_list[0]/10
# b=force_list[1]/10
# tw_cmd1 = geometry_msgs.msg.Twist()
# tw_cmd1.linear.x =-a
# tw_cmd1.linear.y = b
# vel_pub.publish(tw_cmd1)
# rospy.sleep(2.0)
# force_list = force_sensor_capture.get_current_force()
# a=-force_list[0]/10
# b=force_list[1]/10
# tw_cmd2 = geometry_msgs.msg.Twist()
# tw_cmd2.linear.x =-a
# tw_cmd2.linear.y = b
# vel_pub.publish(tw_cmd2)
# rospy.sleep(2.0)
## Move base with linear & Angular motion
# tw = geometry_msgs.msg.Twist()
# tw.linear.x =-1
# tw.angular.z = 0.45
# vel_pub.publish(tw)
# rospy.sleep(4.0)
## Move base with linear & Angular motion second
# tw_cmd1 = geometry_msgs.msg.Twist()
# tw_cmd1.linear.x =-0.6
# tw_cmd1.linear.y =0.2
# tw_cmd1.angular.z = 0.25
# vel_pub.publish(tw_cmd1)
# # rospy.sleep(4.0)
# rospy.sleep(3.0)
# tw_cmd2 = geometry_msgs.msg.Twist()
# tw_cmd2.linear.x =-0.65
# tw_cmd2.linear.y =0.5
# tw_cmd2.angular.z = 0.35
# vel_pub.publish(tw_cmd2)
# # rospy.sleep(4.0)
# rospy.sleep(2.0)
## Open the gripper
gripper.command(1.0)
## Move back
# base.go_rel(-1.3,0.0,start_theta)
gripper.command(1.0)
tw = geometry_msgs.msg.Twist()
tw.linear.x = -1
tw.angular.z = 0.45
vel_pub.publish(tw)
rospy.sleep(2)
whole_body.move_to_neutral()
res.success = True
except Exception as e:
rospy.logerr(e)
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
res.success = False
return res
def _plan_cartesian_path(self, origin_to_pose1, origin_to_pose2,
odom_to_robot_pose, initial_joint_state,
collision_env):
use_joints = (b'wrist_flex_joint', b'wrist_roll_joint',
b'arm_roll_joint', b'arm_flex_joint', b'arm_lift_joint')
req = PlanWithTsrConstraintsRequest()
req.origin_to_basejoint = odom_to_robot_pose
req.initial_joint_state = initial_joint_state
req.base_movement_type.val = BaseMovementType.PLANAR
req.use_joints = use_joints
req.weighted_joints = [b'_linear_base', b'_rotational_base']
req.weight = [self._linear_weight, self._angular_weight]
req.probability_goal_generate = _PLANNING_GOAL_GENERATION
req.attached_objects = []
if collision_env is not None:
req.environment_before_planning = collision_env
req.timeout = rospy.Duration(self._planning_timeout)
req.max_iteration = _PLANNING_MAX_ITERATION
req.uniform_bound_sampling = False
req.deviation_for_bound_sampling = _PLANNING_GOAL_DEVIATION
req.extra_constraints = []
req.extra_goal_constraints = []
move_axis, distance = _movement_axis_and_distance(
origin_to_pose1, origin_to_pose2)
origin_to_axis = _pose_from_x_axis(move_axis)
pose1_to_axis = geometry.multiply_tuples(_invert_pose(origin_to_pose1),
origin_to_axis)
pose1_to_axis = geometry.Pose((0, 0, 0), pose1_to_axis[1])
origin_to_tsr = geometry.multiply_tuples(origin_to_pose1,
pose1_to_axis)
tsr_to_pose1 = _invert_pose(pose1_to_axis)
# Goal constraint
tsr_g = TaskSpaceRegion()
tsr_g.end_frame_id = bytes(self.end_effector_frame)
tsr_g.origin_to_tsr = geometry.tuples_to_pose(origin_to_pose2)
tsr_g.tsr_to_end = geometry.tuples_to_pose(geometry.pose())
tsr_g.min_bounds = [0, 0, 0, 0, 0, 0]
tsr_g.max_bounds = [0, 0, 0, 0, 0, 0]
# Line constraint
tsr_c = TaskSpaceRegion()
tsr_c.end_frame_id = bytes(self.end_effector_frame)
tsr_c.origin_to_tsr = geometry.tuples_to_pose(origin_to_tsr)
tsr_c.tsr_to_end = geometry.tuples_to_pose(tsr_to_pose1)
tsr_c.min_bounds = [0, 0, 0, -math.pi, -math.pi, -math.pi]
tsr_c.max_bounds = [distance, 0, 0, math.pi, math.pi, math.pi]
req.goal_tsrs = [tsr_g]
req.constraint_tsrs = [tsr_c]
service_name = self._setting['plan_with_constraints_service']
plan_service = rospy.ServiceProxy(service_name, PlanWithTsrConstraints)
res = plan_service.call(req)
print(res)
if res.error_code.val != ArmManipulationErrorCodes.SUCCESS:
msg = "Fail to plan"
print(msg)
print(req)
raise exceptions.MotionPlanningError(msg, res.error_code)
return res
def move_cartesian_path(self, waypoints, handpoints, ref_frame_id=None):
if ref_frame_id is None:
ref_frame_id = settings.get_frame('base')
base_frame = settings.get_frame('base')
origin_to_ref_ros_pose = self._lookup_odom_to_ref(ref_frame_id)
##原点のodomからのpose
origin_to_ref = geometry.pose_to_tuples(origin_to_ref_ros_pose)
##原点のodomからのtuple
origin_to_pose1 = self.get_end_effector_pose('odom')
##現在の手先位置
odom_to_robot_pose = self._lookup_odom_to_ref(base_frame)
##足元のodomからのpose
initial_joint_state = self._get_joint_state()
##現在姿勢
if self._collision_world is not None:
collision_env = self._collision_world.snapshot('odom')
else:
collision_env = None
##障害物設定
arm_traj = None
base_traj = None
p = []
for i in range(len(waypoints)):
##各動作点の計算
origin_to_pose2 = geometry.multiply_tuples(origin_to_ref,
waypoints[i])
plan = self._plan_cartesian_path(origin_to_pose1, origin_to_pose2,
odom_to_robot_pose,
initial_joint_state,
collision_env)
p.append(plan)
if arm_traj is None:
arm_traj = plan.solution
arm_traj.points = [
plan.solution.points[0], plan.solution.points[-1]
]
elif len(plan.solution.points) > 0:
arm_traj.points.append(plan.solution.points[-1])
if base_traj is None:
base_traj = plan.base_solution
base_traj.points = [
plan.base_solution.points[0], plan.base_solution.points[-1]
]
elif len(plan.base_solution.points) > 0:
base_traj.points.append(plan.base_solution.points[-1])
origin_to_pose1 = origin_to_pose2
odom_to_robot_pose = RosPose()
final_transform = plan.base_solution.points[-1].transforms[0]
odom_to_robot_pose.position.x = final_transform.translation.x
odom_to_robot_pose.position.y = final_transform.translation.y
odom_to_robot_pose.position.z = final_transform.translation.z
odom_to_robot_pose.orientation.x = final_transform.rotation.x
odom_to_robot_pose.orientation.y = final_transform.rotation.y
odom_to_robot_pose.orientation.z = final_transform.rotation.z
odom_to_robot_pose.orientation.w = final_transform.rotation.w
initial_joint_state = plan.joint_state_after_planning
collision_env = plan.environment_after_planning
base_traj.header.frame_id = settings.get_frame('odom')
constrained_traj = self._constrain_trajectories(arm_traj, base_traj)
constrained_traj.joint_names.append("hand_motor_joint")
if type(constrained_traj.points[0].positions) == tuple:
constrained_traj.points[0].positions = list(
constrained_traj.points[0].positions)
constrained_traj.points[0].velocities = list(
constrained_traj.points[0].velocities)
constrained_traj.points[0].accelerations = list(
constrained_traj.points[0].accelerations)
constrained_traj.points[0].positions.append(handpoints[0])
constrained_traj.points[0].velocities.append(0.0)
if handpoints[0] >= 1.0:
constrained_traj.points[0].accelerations.append(0.1)
elif handpoints[0] <= 0.1:
constrained_traj.points[0].accelerations.append(-0.1)
else:
constrained_traj.points[0].accelerations.append(0.0)
for i in range(len(handpoints)):
constrained_traj.points[i + 1].positions = list(
constrained_traj.points[i + 1].positions)
constrained_traj.points[i + 1].velocities = list(
constrained_traj.points[i + 1].velocities)
constrained_traj.points[i + 1].accelerations = list(
constrained_traj.points[i + 1].accelerations)
constrained_traj.points[i + 1].positions.append(handpoints[i])
constrained_traj.points[i + 1].velocities.append(0.0)
if handpoints[i] >= 1.0:
constrained_traj.points[0].accelerations.append(0.1)
elif handpoints[i] <= 0.1:
constrained_traj.points[0].accelerations.append(-0.1)
else:
constrained_traj.points[0].accelerations.append(0.0)
##この結果の抽出え
self._execute_trajectory(constrained_traj)
return constrained_traj
def main(whole_body, base, gripper, wrist_wrench):
cli = actionlib.SimpleActionClient(
'/hsrb/omni_base_controller/follow_joint_trajectory',
control_msgs.msg.FollowJointTrajectoryAction)
# publisher for delvering command for base move
vel_pub = rospy.Publisher('/hsrb/command_velocity',
geometry_msgs.msg.Twist,
queue_size=10)
# base_pub = rospy.Publisher('/move_base/move/goal',move_base_msgs.msg.MoveBaseActionGoal,queue_size=10)
armPub = rospy.Publisher('/hsrb/arm_trajectory_controller/command',
JointTrajectory,
queue_size=1)
## Grab the handle of door
#test with service - get the handle position from handle
grasp_point_client()
global recog_pos
global Is_found
print recog_pos.pose.position
# target_pose_Msg = rospy.wait_for_message("/handle_detector/grasp_point", PoseStamped)
# recog_pos.pose.position.x=target_pose_Msg.pose.position.x
# recog_pos.pose.position.y=target_pose_Msg.pose.position.y
# recog_pos.pose.position.z=target_pose_Msg.pose.position.z
whole_body.move_to_neutral()
# whole_body.impedance_config= 'grasping'
switch = ImpedanceControlSwitch()
# wrist_wrench.reset()
gripper.command(1.0)
grab_pose = geometry.multiply_tuples(
geometry.pose(x=recog_pos.pose.position.x - HANDLE_TO_HAND_POS_,
y=recog_pos.pose.position.y - 0.012,
z=recog_pos.pose.position.z,
ej=math.pi / 2), geometry.pose(ek=math.pi / 2))
whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF)
wrist_wrench.reset()
# whole_body.impedance_config= 'compliance_middle'
switch.activate("grasping")
# gripper.command(0.01)
gripper.grasp(-0.01)
rospy.sleep(1.0)
switch.inactivate()
wrist_wrench.reset()
rospy.sleep(8.0)
#### test manipulation
whole_body.impedance_config = 'grasping'
# print(whole_body.impedance_config)
# desired_rot=-1.95
# whole_body.move_to_joint_positions({"wrist_roll_joint":desired_rot})
traj = JointTrajectory()
# This controller requires that all joints have values
traj.joint_names = [
"arm_lift_joint", "arm_flex_joint", "arm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
p = JointTrajectoryPoint()
current_positions = [latest_positions[name] for name in traj.joint_names]
current_positions[0] = latest_positions["arm_lift_joint"] - 0.07
current_positions[1] = latest_positions["arm_flex_joint"] - 0.02
current_positions[2] = latest_positions["arm_roll_joint"]
current_positions[3] = latest_positions["wrist_flex_joint"]
current_positions[4] = latest_positions["wrist_roll_joint"] - 0.65
p.positions = current_positions
p.velocities = [0, 0, 0, 0, 0]
p.time_from_start = rospy.Time(3)
traj.points = [p]
armPub.publish(traj)
rospy.sleep(3.0)
# goal_pose =PoseStamped()
# goal_pose.pose.position.x=recog_pos.pose.position.x+0.4
# goal_pose.pose.position.y=recog_pos.pose.position.y+0.2
# goal_pose.pose.position.z=recog_pos.pose.position.z
# goal_pose.pose.orientation.x=recog_pos.pose.orientation.x
# goal_pose.pose.orientation.y=recog_pos.pose.orientation.y
# goal_pose.pose.orientation.z=recog_pos.pose.orientation.z
# goal_pose.pose.orientation.w=recog_pos.pose.orientation.w
# yaw=math.pi/6
# quaternion = tf.transformations.quaternion_from_euler(0.0, 0.0, yaw)
# #type(pose) = geometry_msgs.msg.Pose
# nav_goal = move_base_msgs.msg.MoveBaseActionGoal()
# nav_goal.header.stamp=rospy.Time(0)
# nav_goal.goal.target_pose.header.frame_id = "map"
# nav_goal.goal.target_pose.pose.position.x=recog_pos.pose.position.x+0.3
# nav_goal.goal.target_pose.pose.position.y=recog_pos.pose.position.y+0.2
# nav_goal.goal.target_pose.pose.position.z=0.0
# nav_goal.goal.target_pose.pose.orientation.x=quaternion[0]
# nav_goal.goal.target_pose.pose.orientation.y=quaternion[1]
# nav_goal.goal.target_pose.pose.orientation.z=quaternion[2]
# nav_goal.goal.target_pose.pose.orientation.w=quaternion[3]
# base_pub.publish(nav_goal)
# # whole_body.end_effector_frame = u'odom'
# # whole_body.move_end_effector_by_line((0, 0, 1), -0.2)
# # publish_arm(latest_positions["arm_lift_joint"],latest_positions["arm_flex_joint"],latest_positions["arm_roll_joint"], latest_positions["wrist_flex_joint"],wrist_roll)
whole_body.impedance_config = 'compliance_hard'
whole_body.move_end_effector_by_line((0.0, 0.0, 1), 0.45)
# # whole_body.move_end_effector_by_line((0.7071, 0.7071,0), 0.5)
# # whole_body.move_end_effector_by_line((0, -0.7071, 0.7071), 0.5)
# whole_body.impedance_config= None
tw = geometry_msgs.msg.Twist()
tw.linear.x = 0.9
tw.angular.z = 0.45
vel_pub.publish(tw)
rospy.sleep(4.0)
tw_cmd0 = geometry_msgs.msg.Twist()
tw_cmd0.linear.x = 0.3
tw_cmd0.angular.z = 0.45
vel_pub.publish(tw_cmd0)
# rospy.sleep(4.0)
rospy.sleep(4.0)
gripper.command(1.0)
# whole_body.move_end_effector_by_line((0, 0, -1), 0.25)
tw_cmd = geometry_msgs.msg.Twist()
tw_cmd.linear.x = -1.2
vel_pub.publish(tw_cmd)
rospy.sleep(2.0)
tw_cmd2 = geometry_msgs.msg.Twist()
tw_cmd2.linear.x = -1.1
tw_cmd2.angular.z = -0.4
vel_pub.publish(tw_cmd2)
rospy.sleep(4.0)
whole_body.move_to_neutral()
tw_cmd2 = geometry_msgs.msg.Twist()
tw_cmd2.linear.x = -0.6
tw_cmd2.angular.z = -0.3
vel_pub.publish(tw_cmd2)
def main(whole_body, gripper, wrist_wrench):
rospy.sleep(5)
armPub = rospy.Publisher('/hsrb/arm_trajectory_controller/command',
JointTrajectory,
queue_size=1)
#1.READ THE ANGLE (topic angle_detector)
#angle_Msg = rospy.wait_for_message("angle_detector", Float64)
#angle = angle_Msg.data
#2.READ THE HANDLE POSITION when the door is closed - SERVICE!!!
#target_pose_Msg = rospy.wait_for_message("localize_handle", PoseStamped)
#recog_pos.pose.position.x=target_pose_Msg.pose.position.x
#recog_pos.pose.position.y=target_pose_Msg.pose.position.y
#recog_pos.pose.position.z=target_pose_Msg.pose.position.z
if (angle > -1 and angle < 4): #the door is closed
##3.GRAB THE DOOR HANDLE
whole_body.move_to_neutral()
# whole_body.impedance_config= 'grasping'
switch = ImpedanceControlSwitch()
# wrist_wrench.reset()
gripper.command(1.0)
grab_pose = geometry.multiply_tuples(
geometry.pose(x=recog_pos.pose.position.x - HANDLE_TO_HAND_POS,
y=recog_pos.pose.position.y,
z=recog_pos.pose.position.z,
ej=math.pi / 2), geometry.pose(ek=math.pi / 2))
whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF)
wrist_wrench.reset()
# whole_body.impedance_config= 'compliance_middle'
switch.activate("grasping")
# gripper.command(0.01)
gripper.grasp(-0.008)
rospy.sleep(1.0)
switch.inactivate()
wrist_wrench.reset()
rospy.sleep(8.0)
#### test manipulation
whole_body.impedance_config = 'grasping'
#4.ROTATE HANDLE
# print(whole_body.impedance_config)
# desired_rot=-1.95
# whole_body.move_to_joint_positions({"wrist_roll_joint":desired_rot})
wrist_roll = latest_positions["wrist_roll_joint"] - 0.55
traj = JointTrajectory()
# This controller requires that all joints have values
traj.joint_names = [
"arm_lift_joint", "arm_flex_joint", "arm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
p = JointTrajectoryPoint()
current_positions = [
latest_positions[name] for name in traj.joint_names
]
current_positions[0] = latest_positions["arm_lift_joint"] - 0.04
current_positions[1] = latest_positions["arm_flex_joint"] - 0.015
current_positions[2] = latest_positions["arm_roll_joint"]
current_positions[3] = latest_positions["wrist_flex_joint"]
current_positions[4] = wrist_roll
p.positions = current_positions
p.velocities = [0, 0, 0, 0, 0]
p.time_from_start = rospy.Time(3)
traj.points = [p]
armPub.publish(traj)
rospy.sleep(5.0)
whole_body.impedance_config = 'grasping'
#5.PUSH MOTION - he moves with the door - first option - streight movement
phi = 90 * (2 * math.pi / 360)
l = HANDLE_TO_DOOR_HINGE_POS
d = 2 * l * math.cos(phi)
#open_pose = geometry.pose(x = d*math.sin(phi), y=d*math.cos(phi), ek=math.pi/2 - angle_rad )
#whole_body.move_end_effector_pose(open_pose, _ROBOT_TF)
#angleeee=math.pi/2 - angle_rad
omni_base.go(d * math.sin(phi),
d * math.cos(phi),
math.pi / 2 - angle_rad,
300.0,
relative=True)
whole_body.move_to_neutral()
elif (angle >= 4 and angle < 60): #the door is half-open
#3.ROTATE PARALLEL TO THE DOOR
angle_rad = angle * (2 * math.pi / 360)
#omni_base.go_rel(0.0, 0.0, angle_rad , 300.0)
##4.GRAB THE DOOR HANDLE
whole_body.move_to_neutral()
# whole_body.impedance_config= 'grasping'
switch = ImpedanceControlSwitch()
# wrist_wrench.reset()
gripper.command(0.0)
#change coordinates of the handle - now the door is open so the handle is moved - the data of the handle position are given for the closed door - I m supposing
#that the coordinates of the handle are wrt the base_footprint tf
phi = math.pi / 2 - angle_rad / 2
l = HANDLE_TO_DOOR_HINGE_POS
d1 = 2 * l * math.sin(angle_rad / 2)
recog_pos.pose.position.x = recog_pos.pose.position.x + d1 * math.sin(
phi)
recog_pos.pose.position.y = recog_pos.pose.position.y + d1 * math.cos(
phi)
grab_pose = geometry.multiply_tuples(
geometry.pose(x=recog_pos.pose.position.x - HANDLE_TO_HAND_POS,
y=recog_pos.pose.position.y - 10,
z=recog_pos.pose.position.z,
ej=math.pi / 2), geometry.pose(ek=math.pi / 2))
whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF)
wrist_wrench.reset()
rospy.sleep(1.0)
switch.inactivate()
wrist_wrench.reset()
rospy.sleep(8.0)
#### test manipulation
whole_body.impedance_config = 'grasping'
#5.PUSH MOTION - he moves with the door - first option - streight movement
phi = math.pi / 4 + angle_rad / 2
l = HANDLE_TO_DOOR_HINGE_POS
d = 2 * l * math.cos(phi)
omni_base.go(d * math.sin(phi),
d * math.cos(phi),
math.pi / 2 - angle_rad,
300.0,
relative=True)
def main(whole_body, gripper):
# Grab the handle of door
whole_body.move_to_neutral()
gripper.command(1.0)
grab_pose = geometry.multiply_tuples(geometry.pose(x=HANDLE_POS[0]-HANDLE_TO_HAND_POS,
y=HANDLE_POS[1],
z=HANDLE_POS[2],
ej=math.pi/2),
geometry.pose(ek=math.pi/2))
whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF)
gripper.command(0.1)
rospy.sleep(10.0)
# Rotate the handle (Angle: math.pi/6)
odom_to_hand = get_relative_tuples(_ORIGIN_TF, _HAND_TF)
tsr_to_odom = geometry.pose(x=-(HANDLE_POS[0]-HANDLE_TO_HAND_POS),
y=-(HANDLE_POS[1]-HANDLE_TO_HANDLE_HINGE_POS),
z=-HANDLE_POS[2])
tsr_to_hand = geometry.multiply_tuples(tsr_to_odom, odom_to_hand)
const_tsr = TaskSpaceRegion()
const_tsr.end_frame_id = _HAND_TF
const_tsr.origin_to_tsr = geometry.tuples_to_pose(geometry.pose(x=HANDLE_POS[0]-HANDLE_TO_HAND_POS,
y=HANDLE_POS[1]-HANDLE_TO_HANDLE_HINGE_POS,
z=HANDLE_POS[2]))
const_tsr.tsr_to_end = geometry.tuples_to_pose(tsr_to_hand)
const_tsr.min_bounds = [0, 0.0, 0.0, 0, 0, 0]
const_tsr.max_bounds = [0, 0.0, 0.0, math.pi/6, 0, 0]
goal_tsr = TaskSpaceRegion()
goal_tsr.end_frame_id = _HAND_TF
goal_tsr.origin_to_tsr = geometry.tuples_to_pose(geometry.pose(x=HANDLE_POS[0]-HANDLE_TO_HAND_POS,
y=HANDLE_POS[1]-HANDLE_TO_HANDLE_HINGE_POS,
z=HANDLE_POS[2]))
goal_tsr.tsr_to_end = geometry.tuples_to_pose(tsr_to_hand)
goal_tsr.min_bounds = [0, 0.0, 0.0, math.pi/6, 0, 0]
goal_tsr.max_bounds = [0, 0.0, 0.0, math.pi/6, 0, 0]
response = call_tsr_plan_service(whole_body, [const_tsr], [goal_tsr])
if response.error_code.val != ArmManipulationErrorCodes.SUCCESS:
rospy.logerr("Planning failed: (Error Code {0})".format(response.error_code.val))
exit(-1)
response.base_solution.header.frame_id = _ORIGIN_TF
constrain_traj = whole_body._constrain_trajectories(response.solution,
response.base_solution)
whole_body._execute_trajectory(constrain_traj)
rospy.sleep(10.0)
# Open the door (Angle: math.pi/4)
odom_to_hand = get_relative_tuples(_ORIGIN_TF, _HAND_TF) #T0h
tsr_to_odom = geometry.pose(x=-HANDLE_POS[0],
y=-(HANDLE_POS[1]-HANDLE_TO_DOOR_HINGE_POS),
z=-HANDLE_POS[2]) #Twe
tsr_to_hand = geometry.multiply_tuples(tsr_to_odom, odom_to_hand) #T0s'
const_tsr = TaskSpaceRegion()
const_tsr.end_frame_id = _HAND_TF
const_tsr.origin_to_tsr = geometry.tuples_to_pose(geometry.pose(x=HANDLE_POS[0],
y=HANDLE_POS[1]-HANDLE_TO_DOOR_HINGE_POS,
z=HANDLE_POS[2]))
const_tsr.tsr_to_end = geometry.tuples_to_pose(tsr_to_hand)
const_tsr.min_bounds = [0, 0.0, 0.0, 0, 0, 0]
const_tsr.max_bounds = [0, 0.0, 0.0, 0, 0, math.pi/4]
goal_tsr = TaskSpaceRegion()
goal_tsr.end_frame_id = _HAND_TF
goal_tsr.origin_to_tsr = geometry.tuples_to_pose(geometry.pose(x=HANDLE_POS[0],
y=HANDLE_POS[1]-HANDLE_TO_DOOR_HINGE_POS,
z=HANDLE_POS[2]))
goal_tsr.tsr_to_end = geometry.tuples_to_pose(tsr_to_hand)
goal_tsr.min_bounds = [0, 0.0, 0.0, 0, 0, math.pi/4]
goal_tsr.max_bounds = [0, 0.0, 0.0, 0, 0, math.pi/4]
response = call_tsr_plan_service(whole_body, [const_tsr], [goal_tsr])
if response.error_code.val != ArmManipulationErrorCodes.SUCCESS:
rospy.logerr("Planning failed: (Error Code {0})".format(response.error_code.val))
exit(-1)
response.base_solution.header.frame_id = _ORIGIN_TF
constrain_traj = whole_body._constrain_trajectories(response.solution,
response.base_solution)
whole_body._execute_trajectory(constrain_traj)
def opendoor(req):
# main(whole_body, gripper,wrist_wrench)
frame = req.handle_pose.header.frame_id
hanlde_pos = req.handle_pose.pose
# hanlde_pos=geometry_msgs.msg.PoseStamped()
res = OpendoorResponse()
armPub = rospy.Publisher('/hsrb/arm_trajectory_controller/command',
JointTrajectory,
queue_size=1)
robot = hsrb_interface.Robot()
whole_body = robot.get('whole_body')
gripper = robot.get('gripper')
wrist_wrench = robot.get('wrist_wrench')
# with hsrb_interface.Robot() as robot:
# whole_body = robot.get('whole_body')
# gripper = robot.get('gripper')
# wrist_wrench = robot.get('wrist_wrench')
try:
# Open the gripper
print("Opening the gripper")
whole_body.move_to_neutral()
switch = ImpedanceControlSwitch()
gripper.command(1.0)
# Approach to the door
print("Approach to the door")
grab_pose = geometry.multiply_tuples(
geometry.pose(x=hanlde_pos.position.x - HANDLE_TO_HAND_POS,
y=hanlde_pos.position.y,
z=hanlde_pos.position.z,
ej=math.pi / 2), geometry.pose(ek=math.pi / 2))
whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF)
# Close the gripper
wrist_wrench.reset()
switch.activate("grasping")
gripper.grasp(-0.008)
rospy.sleep(1.0)
switch.inactivate()
# Rotate the handle
whole_body.impedance_config = 'grasping'
traj = JointTrajectory()
# This controller requires that all joints have values
traj.joint_names = [
"arm_lift_joint", "arm_flex_joint", "arm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
p = JointTrajectoryPoint()
# set the desired value
current_positions = [
latest_positions[name] for name in traj.joint_names
]
current_positions[0] = latest_positions["arm_lift_joint"] - 0.04
current_positions[1] = latest_positions["arm_flex_joint"] - 0.015
current_positions[2] = latest_positions["arm_roll_joint"]
current_positions[3] = latest_positions["wrist_flex_joint"]
current_positions[4] = latest_positions["wrist_roll_joint"] - 0.55
p.positions = current_positions
p.velocities = [0, 0, 0, 0, 0]
p.time_from_start = rospy.Time(3)
traj.points = [p]
armPub.publish(traj)
rospy.sleep(5.0)
whole_body.impedance_config = 'grasping'
whole_body.move_end_effector_by_line((0, 0, 1), 0.35)
whole_body.impedance_config = None
gripper.command(1.0)
whole_body.move_to_neutral()
res.success = True
except Exception as e:
rospy.logerr(e)
print "Failed to open door"
res.success = False
return res
def main(whole_body, gripper):
# Grab the handle of door
whole_body.move_to_neutral()
gripper.command(1.0) #open hand
grab_pose = geometry.multiply_tuples(geometry.pose(x=HANDLE_POS[0]-HANDLE_TO_HAND_POS,
y=HANDLE_POS[1],
z=HANDLE_POS[2],
ej=(math.pi/2)),
geometry.pose(ek=-(math.pi/2)))
whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF) #the frame that is used is specified
gripper.grasp(-1.0) #close hand
rospy.sleep(10.0)
# Rotate the handle (Angle: math.pi/6)
odom_to_hand = get_relative_tuples(_ORIGIN_TF, _HAND_TF)
tsr_to_odom = geometry.pose(x=-(HANDLE_POS[0]-HANDLE_TO_HAND_POS),
y=-(HANDLE_POS[1]+HANDLE_TO_HANDLE_HINGE_POS),
z=-HANDLE_POS[2])
tsr_to_hand = geometry.multiply_tuples(tsr_to_odom, odom_to_hand)
#creation of constraints
const_tsr = TaskSpaceRegion()
const_tsr.end_frame_id = _HAND_TF #I'm in the hand frame
const_tsr.origin_to_tsr = geometry.tuples_to_pose(geometry.pose(x=HANDLE_POS[0]-HANDLE_TO_HAND_POS,
y=HANDLE_POS[1]+HANDLE_TO_HANDLE_HINGE_POS,
z=HANDLE_POS[2]))
const_tsr.tsr_to_end = geometry.tuples_to_pose(tsr_to_hand)
const_tsr.min_bounds = [0, 0.0, 0.0, -(math.pi/6), 0, 0]
const_tsr.max_bounds = [0, 0.0, 0.0, 0, 0, 0]
#creation of the goal position
goal_tsr = TaskSpaceRegion()
goal_tsr.end_frame_id = _HAND_TF #I'm in the hand frame
goal_tsr.origin_to_tsr = geometry.tuples_to_pose(geometry.pose(x=HANDLE_POS[0]-HANDLE_TO_HAND_POS,
y=HANDLE_POS[1]+HANDLE_TO_HANDLE_HINGE_POS,
z=HANDLE_POS[2]))
goal_tsr.tsr_to_end = geometry.tuples_to_pose(tsr_to_hand)
goal_tsr.min_bounds = [0, 0.0, 0.0, -(math.pi/6), 0, 0]
goal_tsr.max_bounds = [0, 0.0, 0.0, -(math.pi/6), 0, 0]
#computation of the trajectory
response = call_tsr_plan_service(whole_body, [const_tsr], [goal_tsr])
#make sure no errors
if response.error_code.val != ArmManipulationErrorCodes.SUCCESS:
rospy.logerr("Planning failed: (Error Code {0})".format(response.error_code.val))
exit(-1)
response.base_solution.header.frame_id = _ORIGIN_TF
constrain_traj = whole_body._constrain_trajectories(response.solution,
response.base_solution)
#EXECUTION of the handle rotation
whole_body._execute_trajectory(constrain_traj)
rospy.sleep(10.0)
#at this point the handle should be open
# Open the door (Angle: math.pi/4)
odom_to_hand = get_relative_tuples(_ORIGIN_TF, _HAND_TF)
tsr_to_odom = geometry.pose(x=-HANDLE_POS[0],
y=-(HANDLE_POS[1]+HANDLE_TO_DOOR_HINGE_POS),
z=-HANDLE_POS[2])
tsr_to_hand = geometry.multiply_tuples(tsr_to_odom, odom_to_hand)
#constraints definition
const_tsr = TaskSpaceRegion()
const_tsr.end_frame_id = _HAND_TF #frame i am modifying
const_tsr.origin_to_tsr = geometry.tuples_to_pose(geometry.pose(x=HANDLE_POS[0],
y=HANDLE_POS[1]+HANDLE_TO_DOOR_HINGE_POS,
z=HANDLE_POS[2]))
const_tsr.tsr_to_end = geometry.tuples_to_pose(tsr_to_hand)
const_tsr.min_bounds = [0, 0.0, 0.0, 0, 0, 0]
const_tsr.max_bounds = [0, 0.0, 0.0, 0, 0, (math.pi/2)]
#definition of final position
goal_tsr = TaskSpaceRegion()
goal_tsr.end_frame_id = _HAND_TF #frame i am modifying
goal_tsr.origin_to_tsr = geometry.tuples_to_pose(geometry.pose(x=HANDLE_POS[0],
y=HANDLE_POS[1]+HANDLE_TO_DOOR_HINGE_POS,
z=HANDLE_POS[2]))
goal_tsr.tsr_to_end = geometry.tuples_to_pose(tsr_to_hand)
goal_tsr.min_bounds = [0, 0.0, 0.0, 0, 0, (math.pi/2)]
goal_tsr.max_bounds = [0, 0.0, 0.0, 0, 0, (math.pi/2)]
#trajectory planning
response = call_tsr_plan_service(whole_body, [const_tsr], [goal_tsr])
#make sure no errors
if response.error_code.val != ArmManipulationErrorCodes.SUCCESS:
rospy.logerr("Planning failed: (Error Code {0})".format(response.error_code.val))
exit(-1)
response.base_solution.header.frame_id = _ORIGIN_TF
constrain_traj = whole_body._constrain_trajectories(response.solution,
response.base_solution)
#execution of results
whole_body._execute_trajectory(constrain_traj)
