def spin(self):
print "creating interface for Velma..."
# create the interface for Velma robot
velma = Velma()
rospy.sleep(0.5)
print "done."
hv = [1.2, 1.2, 1.2, 1.2]
ht = [3000, 3000, 3000, 3000]
velma.moveHandLeft([15.0/180.0*math.pi, 15.0/180.0*math.pi, 15.0/180.0*math.pi, 0], hv, ht, 5000, True)
print "wainting for moveHandLeft complete..."
result = velma.waitForHandLeft()
print "result:", result.error_code
velma.moveHandRight([15.0/180.0*math.pi, 15.0/180.0*math.pi, 15.0/180.0*math.pi, 0], hv, ht, 5000, True)
print "wainting for moveHandRight complete..."
result = velma.waitForHandRight()
print "result:", result.error_code
velma.moveHandLeft([70.0/180.0*math.pi, 70.0/180.0*math.pi, 70.0/180.0*math.pi, 0], hv, ht, 5000, True)
print "wainting for moveHandLeft complete..."
result = velma.waitForHandLeft()
print "result:", result.error_code
velma.switchToCart()
velma.updateTransformations()
T_B_Wr1 = velma.T_B_Wr
T_B_Wr2 = PyKDL.Frame(PyKDL.Vector(0.2,0,0)) * T_B_Wr1
velma.moveWristRight(T_B_Wr2, 3.0, Wrench(Vector3(40,40,40), Vector3(14,14,14)), start_time=0.1)
print "wainting for moveWristRight complete..."
result = velma.waitForWristRight()
print "result:", result.error_code
velma.moveWristRight(T_B_Wr1, 3.0, Wrench(Vector3(40,40,40), Vector3(14,14,14)), start_time=0.1)
print "wainting for moveWristRight complete..."
result = velma.waitForWristRight()
print "result:", result.error_code
velma.updateTransformations()
T_B_Wl1 = velma.T_B_Wl
T_B_Wl2 = PyKDL.Frame(PyKDL.Vector(0,0,0.2)) * T_B_Wl1
velma.moveWristLeft(T_B_Wl2, 3.0, Wrench(Vector3(40,40,40), Vector3(14,14,14)), start_time=0.1)
print "wainting for moveWristLeft complete..."
result = velma.waitForWristLeft()
print "result:", result.error_code
velma.moveWristLeft(T_B_Wl1, 3.0, Wrench(Vector3(40,40,40), Vector3(14,14,14)), start_time=0.1)
print "wainting for moveWristLeft complete..."
result = velma.waitForWristLeft()
print "result:", result.error_code
raw_input("Press ENTER to exit...")
def spin(self):
sys.setrecursionlimit(200)
# generate the set of grasps for a jar
grasps_T_J_E = []
for angle_jar_axis in np.arange(0.0, math.pi*2.0, 10.0/180.0*math.pi):
for translation_jar_axis in np.linspace(-0.03, 0.03, 7):
T_J_E = PyKDL.Frame(PyKDL.Rotation.RotZ(90.0/180.0*math.pi)) * PyKDL.Frame(PyKDL.Rotation.RotX(angle_jar_axis)) * PyKDL.Frame(PyKDL.Vector(translation_jar_axis, 0, -0.17))
grasps_T_J_E.append( (T_J_E, T_J_E.Inverse()) )
T_J_E = PyKDL.Frame(PyKDL.Rotation.RotZ(-90.0/180.0*math.pi)) * PyKDL.Frame(PyKDL.Rotation.RotX(angle_jar_axis)) * PyKDL.Frame(PyKDL.Vector(translation_jar_axis, 0, -0.17))
grasps_T_J_E.append( (T_J_E, T_J_E.Inverse()) )
# TEST: OpenJarTaskRRT
if False:
task = tasks.OpenJarTaskRRT(None, ("right", grasps_T_J_E, ))
m_id = 0
for coord in task.valid:
xi,yi,zi = coord
x = plutl.x_set[xi]
y = plutl.y_set[yi]
z = plutl.z_set[zi]
rot_set = task.valid[coord]
size = float(len(rot_set))/40.0
m_id = self.pub_marker.publishSinglePointMarker(PyKDL.Vector(x,y,z), m_id, r=1.0, g=1*size, b=1*size, a=1, namespace='default', frame_id="torso_link2", m_type=Marker.SPHERE, scale=Vector3(0.05*size, 0.05*size, 0.05*size))
rospy.sleep(0.01)
exit(0)
rospack = rospkg.RosPack()
env_file=rospack.get_path('velma_scripts') + '/data/common/velma_room.env.xml'
# env_file=rospack.get_path('velma_scripts') + '/data/common/velma_room_obstacles.env.xml'
srdf_path=rospack.get_path('velma_description') + '/robots/'
obj_filenames = [
rospack.get_path('velma_scripts') + '/data/jar/jar.kinbody.xml'
]
rrt = rrt_star_connect_planner.PlannerRRT(3, env_file, obj_filenames, srdf_path)
rospy.wait_for_service('/change_state')
changeState = rospy.ServiceProxy('/change_state', state_server_msgs.srv.ChangeState)
self.listener = tf.TransformListener()
print "creating interface for Velma..."
# create the interface for Velma robot
velma = Velma()
self.pub_head_look_at = rospy.Publisher("/head_lookat_pose", geometry_msgs.msg.Pose)
print "done."
rospy.sleep(0.5)
velma.updateTransformations()
# switch to cartesian impedance mode...
# velma.switchToCart()
# exit(0)
velma.switchToJoint()
# TEST: moving in joint impedance mode
if False:
raw_input("Press ENTER to move the robot...")
velma.switchToJoint()
joint_names = [ "right_arm_4_joint" ]
q_dest = [ velma.js_pos["right_arm_4_joint"] - 10.0/180.0*math.pi ]
velma.moveJoint(q_dest, joint_names, 10.0, start_time=0.5)
traj = [[q_dest], None, None, [10.0]]
velma.moveJointTraj(traj, joint_names, start_time=0.5)
result = velma.waitForJoint()
print "moveJointTraj result", result.error_code
exit(0)
hv = [1.2, 1.2, 1.2, 1.2]
ht = [3000, 3000, 3000, 3000]
target_gripper = "right"
if target_gripper == "left":
velma.moveHandLeft([40.0/180.0*math.pi, 40.0/180.0*math.pi, 40.0/180.0*math.pi, 0], hv, ht, 5000, True)
velma.moveHandRight([120.0/180.0*math.pi, 120.0/180.0*math.pi, 120.0/180.0*math.pi, 0], hv, ht, 5000, True)
else:
velma.moveHandLeft([120.0/180.0*math.pi, 120.0/180.0*math.pi, 120.0/180.0*math.pi, 0], hv, ht, 5000, True)
velma.moveHandRight([40.0/180.0*math.pi, 40.0/180.0*math.pi, 40.0/180.0*math.pi, 0], hv, ht, 5000, True)
result = velma.waitForHandLeft()
print "waitForHandLeft result", result.error_code
result = velma.waitForHandRight()
print "waitForHandRight result", result.error_code
#
# Initialise Openrave
#
openrave = openraveinstance.OpenraveInstance()
openrave.startOpenrave(collision='fcl')
openrave.loadEnv(env_file)
openrave.runOctomapClient(ros=True)
openrave.readRobot(srdf_path=srdf_path)
for filename in obj_filenames:
body = openrave.env.ReadKinBodyXMLFile(filename)
body.Enable(False)
body.SetVisible(False)
openrave.env.Add(body)
mo_state = objectstate.MovableObjectsState(openrave.env, obj_filenames)
openrave.setCamera(PyKDL.Vector(2.0, 0.0, 2.0), PyKDL.Vector(0.60, 0.0, 1.10))
openrave.updateRobotConfigurationRos(velma.js_pos)
while True:
if self.listener.canTransform('torso_base', 'jar', rospy.Time(0)):
pose = self.listener.lookupTransform('torso_base', 'jar', rospy.Time(0))
T_B_J = pm.fromTf(pose)
break
print "waiting for planner..."
rrt.waitForInit()
print "planner initialized"
T_B_E_list = []
for T_J_E, T_E_J in grasps_T_J_E:
T_B_Ed = T_B_J * T_J_E
T_B_E_list.append(T_B_Ed)
print "grasps for jar:", len(T_B_E_list)
if True:
# look around
self.pub_head_look_at.publish(pm.toMsg(PyKDL.Frame(PyKDL.Vector(1,0,1.2))))
rospy.sleep(3)
# self.pub_head_look_at.publish(pm.toMsg(PyKDL.Frame(PyKDL.Vector(1,-1,1.2))))
# rospy.sleep(2)
# self.pub_head_look_at.publish(pm.toMsg(PyKDL.Frame(PyKDL.Vector(1,1,1.2))))
# rospy.sleep(2)
# self.pub_head_look_at.publish(pm.toMsg(PyKDL.Frame(PyKDL.Vector(1,0,1.2))))
# rospy.sleep(2)
raw_input("Press ENTER to start planning...")
openrave.updateOctomap()
tree_serialized = openrave.or_octomap_client.SendCommand("GetOcTree")
for i in range(10):
time_tf = rospy.Time.now()-rospy.Duration(0.5)
mo_state.update(self.listener, time_tf)
mo_state.updateOpenrave(openrave.env)
rospy.sleep(0.1)
print "octomap updated"
print "Planning trajectory to grasp the jar..."
env_state = (openrave.robot_rave.GetDOFValues(), mo_state.obj_map, tree_serialized, [])
path, dof_names = rrt.RRTstar(env_state, tasks.GraspTaskRRT, (target_gripper, T_B_E_list), 240.0)
traj = []
for i in range(len(path)-1):
q1 = path[i]
q2 = path[i+1]
for f in np.linspace(0.0, 1.0, 40):
traj.append( q1 * (1.0 - f) + q2 * f )
while True:
if raw_input("Type e to execute") == 'e':
break
openrave.showTrajectory(dof_names, 10.0, traj)
traj = velma.prepareTrajectory(path, velma.getJointStatesByNames(dof_names), dof_names, speed_mult=1.0)
velma.switchToJoint()
velma.moveJointTraj(traj, dof_names, start_time=0.5)
result = velma.waitForJoint()
print "moveJointTraj result", result.error_code
if result.error_code != 0:
exit(0)
raw_input("Press ENTER to continue...")
openrave.updateRobotConfigurationRos(velma.js_pos)
print "before grasp"
report = CollisionReport()
if openrave.env.CheckCollision(openrave.robot_rave, report):
print "collision"
else:
print "no collision"
report = CollisionReport()
if openrave.robot_rave.CheckSelfCollision(report):
print "self-collision"
else:
print "no self-collision"
raw_input("Press ENTER to close the gripper...")
if target_gripper == "left":
velma.moveHandLeft([90.0/180.0*math.pi, 90.0/180.0*math.pi, 90.0/180.0*math.pi, 0], hv, ht, 5000, True)
result = velma.waitForHandLeft()
print "moveHandLeft result", result.error_code
else:
velma.moveHandRight([90.0/180.0*math.pi, 90.0/180.0*math.pi, 90.0/180.0*math.pi, 0], hv, ht, 5000, True)
result = velma.waitForHandRight()
print "waitForHandRight result", result.error_code
openrave.updateRobotConfigurationRos(velma.js_pos)
print "after grasp"
report = CollisionReport()
if openrave.env.CheckCollision(openrave.robot_rave, report):
print "collision"
else:
print "no collision"
report = CollisionReport()
if openrave.robot_rave.CheckSelfCollision(report):
print "self-collision"
else:
print "no self-collision"
openrave.robot_rave.Grab(openrave.env.GetKinBody("jar"), openrave.robot_rave.GetLink(target_gripper+"_HandPalmLink"))
changeState("grasp "+target_gripper+"_HandPalmLink "+ "jar")
print "after grab"
report = CollisionReport()
if openrave.env.CheckCollision(openrave.robot_rave, report):
print "collision"
else:
print "no collision"
report = CollisionReport()
if openrave.robot_rave.CheckSelfCollision(report):
print "self-collision"
else:
print "no self-collision"
print "Planning trajectory to pull the jar from the cabinet..."
for i in range(10):
time_tf = rospy.Time.now()-rospy.Duration(0.5)
mo_state.update(self.listener, time_tf)
mo_state.updateOpenrave(openrave.env)
rospy.sleep(0.1)
env_state = (openrave.robot_rave.GetDOFValues(), mo_state.obj_map, tree_serialized, [["jar", target_gripper+"_HandPalmLink"]])
# path, dof_names = rrt.RRTstar(env_state, tasks.GraspTaskRRT, (target_gripper, T_B_E_list), 60.0)
path, dof_names = rrt.RRTstar(env_state, tasks.MoveArmsCloseTaskRRT, (None,), 120.0)
# path.reverse()
traj = []
for i in range(len(path)-1):
q1 = path[i]
q2 = path[i+1]
for f in np.linspace(0.0, 1.0, 40):
traj.append( q1 * (1.0 - f) + q2 * f )
while True:
if raw_input("Type e to execute") == 'e':
break
openrave.showTrajectory(dof_names, 10.0, traj)
traj = velma.prepareTrajectory(path, velma.getJointStatesByNames(dof_names), dof_names, speed_mult=1.0)
result = velma.moveJointTraj(traj, dof_names, start_time=0.5)
result = velma.waitForJoint()
print "moveJointTraj result", result.error_code
if result.error_code != 0:
exit(0)
raw_input("Press ENTER to continue...")
openrave.updateRobotConfigurationRos(velma.js_pos)
changeState("drop "+target_gripper+"_HandPalmLink")
raw_input("Press ENTER to exit...")
rrt.cleanup()
class TestOrOctomap:
"""
"""
def __init__(self):
self.pub_marker = velmautils.MarkerPublisher()
def spin(self):
simulation = True
rospack = rospkg.RosPack()
env_file=rospack.get_path('velma_scripts') + '/data/key/vis_test.env.xml'
xacro_uri=rospack.get_path('velma_description') + '/robots/velma.urdf.xacro'
srdf_path=rospack.get_path('velma_description') + '/robots/'
rrt = rrt_star_connect_planner.PlannerRRT(3, env_file, xacro_uri, srdf_path)
print "creating interface for Velma..."
# create the interface for Velma robot
self.velma = Velma()
print "done."
rospy.sleep(0.5)
self.velma.updateTransformations()
if simulation:
hv = [3.2, 3.2, 3.2, 3.2]
ht = [3000, 3000, 3000, 3000]
self.velma.moveHandLeft([120.0/180.0*math.pi, 120.0/180.0*math.pi, 120.0/180.0*math.pi, 0], hv, ht, 5000, True)
self.velma.moveHandRight([120.0/180.0*math.pi, 120.0/180.0*math.pi, 120.0/180.0*math.pi, 0], hv, ht, 5000, True)
rospy.sleep(1.0)
#
# Initialise Openrave
#
openrave = openraveinstance.OpenraveInstance()
openrave.startOpenraveURDF(env_file=env_file)
openrave.readRobot(xacro_uri=xacro_uri, srdf_path=srdf_path)
rrt.waitForInit()
openrave.setCamera(PyKDL.Vector(2.0, 0.0, 2.0), PyKDL.Vector(0.60, 0.0, 1.10))
openrave.updateRobotConfigurationRos(self.velma.js_pos)
# TEST: key hole goal
if False:
task = KeyRotTaskRRT(openrave)
task.SampleGoal(None, None)
exit(0)
# TEST: head IK
if False:
v_rot = 0.800
v_lean = 0.375
v_head = 0.392
h_cam = 0.0
v_cam = 0.225
head_kin = headkinematics.HeadKinematics(v_rot, v_lean, v_head, h_cam, v_cam)
openrave.addSphere("target_sphere", 0.05)
while not rospy.is_shutdown():
head_kin.UpdateTorsoPose(openrave.robot_rave.GetJoint("torso_0_joint").GetValue(0), openrave.robot_rave.GetJoint("torso_1_joint").GetValue(0))
target_pos = PyKDL.Vector(1, random.uniform(-1.5, 1.5), random.uniform(0,2))
head_kin.UpdateTargetPosition(target_pos.x(), target_pos.y(), target_pos.z())
openrave.updatePose("target_sphere", PyKDL.Frame(target_pos))
head_kin.TransformTargetToHeadFrame()
joint_pan, joint_tilt = head_kin.CalculateHeadPose()
if joint_pan == None:
continue
openrave.robot_rave.SetDOFValues([joint_pan, joint_tilt], [openrave.robot_rave.GetJoint("head_pan_joint").GetDOFIndex(), openrave.robot_rave.GetJoint("head_tilt_joint").GetDOFIndex()])
raw_input("Press ENTER to continue...")
exit(0)
raw_input("Press ENTER to continue...")
# T_B_E_list = [PyKDL.Frame(PyKDL.Rotation.RotY(90.0/180.0*math.pi), PyKDL.Vector(0.6, 0, 1.6))]
# path, dof_names = rrt.RRTstar(openrave.robot_rave.GetDOFValues(), tasks.GraspTaskRRT, ("right", T_B_E_list), 30.0)
path, dof_names = rrt.RRTstar(openrave.robot_rave.GetDOFValues(), tasks.KeyRotTaskRRT, ("right",), 30.0)
# path, dof_names = rrt.RRTstar(openrave.robot_rave.GetDOFValues(), tasks.LooAtTaskRRT, ("right",), 60.0)
# path, dof_names = rrt.RRTstar(openrave.robot_rave.GetDOFValues(), tasks.MoveArmsCloseTaskRRT, ("right",), 30.0)
traj = []
for i in range(len(path)-1):
q1 = path[i]
q2 = path[i+1]
for f in np.linspace(0.0, 1.0, 40):
traj.append( q1 * (1.0 - f) + q2 * f )
while True:
if raw_input("Type e to exit") == 'e':
break
openrave.showTrajectory(dof_names, 10.0, traj)
rrt.cleanup()
# raw_input("Press ENTER to exit...")
exit(0)
rospy.sleep(1)
openrave.runOctomap()
sphere = RaveCreateKinBody(openrave.env,'')
sphere.SetName("sphere")
sphere.InitFromSpheres(numpy.array([[0,0,0,0.05]]),True)
openrave.env.Add(sphere,True)
# test the collision checker for octomap
if True:
raw_input("Press ENTER to continue...")
ob = openrave.env.GetKinBody("_OCTOMAP_MAP_")
cc = openrave.env.GetCollisionChecker()
m_id = 0
for x in np.linspace(0,1.5,30):
for y in np.linspace(-1,1,40):
for z in np.linspace(1,2,20):
# print x,y,z
tr = conv.KDLToOpenrave(PyKDL.Frame(PyKDL.Vector(x,y,z)))
sphere.SetTransform(tr)
openrave.env.UpdatePublishedBodies()
report = CollisionReport()
ret = cc.CheckCollision(sphere, report)
# ret = openrave.env.CheckCollision(ob, report)
# print ret
if ret:
m_id = self.pub_marker.publishSinglePointMarker(PyKDL.Vector(x,y,z), m_id, r=1, g=0, b=0, a=1, namespace='default', frame_id='world', m_type=Marker.SPHERE, scale=Vector3(0.1, 0.1, 0.1), T=None)
continue
if report.plink1 == None:
print None
else:
print report.plink1.GetParent().GetName(), report.plink2.GetName()
# print " ", report.vLinkColliding
for link1, link2 in report.vLinkColliding:
print " ", link1.GetParent().GetName(), link2.GetName()
# print report.plink1.GetParent().GetName(), report.plink2.GetParent().GetName()
exit(0)
self.pub_head_look_at = rospy.Publisher("/head_lookat_pose", geometry_msgs.msg.Pose)
raw_input("Press ENTER to look around...")
# self.pub_head_look_at.publish(pm.toMsg(PyKDL.Frame(PyKDL.Vector(0.2,-0.5,1))))
# raw_input("Press ENTER to exit...")
# exit(0)
raw_input("Press ENTER to look around...")
self.pub_head_look_at.publish(pm.toMsg(PyKDL.Frame(PyKDL.Vector(1,0,2))))
raw_input("Press ENTER to look around...")
self.pub_head_look_at.publish(pm.toMsg(PyKDL.Frame(PyKDL.Vector(0.2,1,2))))
raw_input("Press ENTER to look around...")
self.pub_head_look_at.publish(pm.toMsg(PyKDL.Frame(PyKDL.Vector(0.2,1,1.2))))
raw_input("Press ENTER to look around...")
self.pub_head_look_at.publish(pm.toMsg(PyKDL.Frame(PyKDL.Vector(1,0,1.2))))
raw_input("Press ENTER to look around...")
self.pub_head_look_at.publish(pm.toMsg(PyKDL.Frame(PyKDL.Vector(0.2,-1,1.2))))
raw_input("Press ENTER to look around...")
self.pub_head_look_at.publish(pm.toMsg(PyKDL.Frame(PyKDL.Vector(0.2,-1,2))))
raw_input("Press ENTER to look around...")
self.pub_head_look_at.publish(pm.toMsg(PyKDL.Frame(PyKDL.Vector(1,0,2))))
raw_input(".")
exit(0)
def spin(self):
# create the robot interface
velma = Velma()
velma.updateTransformations()
# reset the gripper
velma.resetFingersLeft()
velma.resetFingersRight()
rospy.sleep(2)
velma.moveHandRight([70.0/180.0*math.pi, 70.0/180.0*math.pi, 70.0/180.0*math.pi, 70.0/180.0*math.pi], [1.2, 1.2, 1.2, 1.2], [4000, 4000, 4000, 4000], 4000, hold=True)
rospy.sleep(3)
return
# create the jar model
jar = Jar(self.pub_marker)
# start thread for jar tf publishing and for visualization
thread.start_new_thread(jar.tfBroadcasterLoop, (0.5, 1))
# look for jar marker
T_B_J = self.getJarMarkerPose()
if T_B_J == None:
print "Cound not find jar marker."
exit(0)
# jar marker is found, add observation to the jar model
print "Found jar marker."
jar.addMarkerObservation(T_B_J)
# calculate angle between jar_axis and vertical vector (z) in B
jar_up_angle = 180.0*getAngle(PyKDL.Frame(T_B_J.M)*PyKDL.Vector(0,0,1), PyKDL.Vector(0,0,1))/math.pi
print "angle between jar_axis and vertical vector (z) in B: %s deg."%(jar_up_angle)
# stiffness for jar touching
k_jar_touching = Wrench(Vector3(1200.0, 1200.0, 1200.0), Vector3(300.0, 300.0, 300.0))
k_jar_cap_gripping = Wrench(Vector3(1200.0, 1200.0, 1200.0), Vector3(300.0, 300.0, 300.0))
k_jar_cap_rotating = Wrench(Vector3(100.0, 100.0, 1200.0), Vector3(300.0, 300.0, 300.0))
# create the robot interface
velma = Velma()
velma.updateTransformations()
# reset the gripper
self.resetGripper(velma)
# start with very low stiffness
print "setting stiffness to very low value"
velma.moveImpedance(velma.k_error, 0.5)
if velma.checkStopCondition(0.5):
exit(0)
raw_input("Press Enter to continue...")
if velma.checkStopCondition():
exit(0)
velma.updateTransformations()
velma.updateAndMoveTool( velma.T_W_E, 1.0 )
if velma.checkStopCondition(1.0):
exit(0)
raw_input("Press Enter to continue...")
print "setting stiffness to bigger value"
velma.moveImpedance(k_jar_touching, 3.0)
if velma.checkStopCondition(3.0):
exit(0)
if False:
velma.moveAroundQ5Singularity(5.0/180.0*math.pi)
print "aborted_on_q5_singularity: %s"%(velma.aborted_on_q5_singularity)
exit(0)
if False:
velma.updateTransformations()
i = 6
omega = 5.0/180.0*math.pi
if velma.q_r[i] > 0.0:
angle = -0.0/180.0*math.pi
omega = -math.fabs(omega)
else:
angle = 0.0/180.0*math.pi
omega = math.fabs(omega)
traj, times = velma.generateTrajectoryInJoint(i, -velma.q_r[i]+angle, omega)
velma.moveWrist2(traj[-1]*velma.T_W_T)
raw_input("Press Enter to move the robot in " + str(times[-1]) + " s...")
if velma.checkStopCondition():
exit(0)
velma.moveWristTraj(traj, times, Wrench(Vector3(20,20,20), Vector3(4,4,4)), abort_on_q5_singularity=False)
velma.checkStopCondition(times[-1]+0.5)
exit(0)
velma.calibrateTactileSensors()
print "generating grasps..."
# calculate the best grip for the jar cap
jarCapForceGrasps = self.generateGrasps_jarCapForce(velma, jar, 70.0/180.0*math.pi)
print "generated %s grasps: %s"%(len(jarCapForceGrasps), jarCapForceGrasps[0].name)
# calculate the best grip for the jar side touching
jarCapPinchGrasps = self.generateGrasps_jarCapPinch(velma, jar)
print "generated %s grasps: %s"%(len(jarCapPinchGrasps), jarCapPinchGrasps[0].name)
# calculate the best grip for the jar top touching
jarCapTopTouchGrasps = self.GenerateGrasps_jarCapTopTouch(velma)
print "generated %s grasps: %s"%(len(jarCapTopTouchGrasps), jarCapTopTouchGrasps[0].name)
self.grasps = jarCapForceGrasps + jarCapPinchGrasps + jarCapTopTouchGrasps
print "Total %s grasps"%(len(self.grasps))
# get contact points observations
# we want to touch the jar's cap with the middle of the 5th row of tactile matrix
if True:
actions = []
actions.append(JarLidTopTouchAction(velma))
actions.append(JarLidPinchGraspAction(velma))
actions.append(RemoveJarLidAction(velma))
actions.append(RemoveJarLidAction(velma))
actions.append(RemoveJarLidAction(velma))
actions.append(RemoveJarLidAction(velma))
actions.append(RemoveJarLidAction(velma))
self.resetContacts()
jar.resetContactObservations()
action_index = 0
last_k = copy.deepcopy(k_jar_touching)
last_q = [0.0, 0.0, 0.0, 0.0]
for action in actions:
# get the fresh pose of the jar
T_B_JC = copy.deepcopy(jar.getJarCapFrame())
action.setTarget(T_B_JC)
stop = False
action.recalculateRelations(self.grasps)
rel_index = 0
while not stop:
rel = action.relations[rel_index]
print "rel: %s"%(rel.name)
if rel.grasp.q_pre != None and (last_q[0] != rel.grasp.q_pre[0] or last_q[1] != rel.grasp.q_pre[1] or last_q[2] != rel.grasp.q_pre[2] or last_q[3] != rel.grasp.q_pre[3]):
last_q = copy.deepcopy(rel.grasp.q_pre)
velma.move_hand_client("right", rel.grasp.q_pre, t=rel.grasp.t_pre )
if velma.checkStopCondition(3.0):
exit(0)
if rel.k_pre != None and (last_k.force.x != rel.k_pre.force.x or last_k.force.y != rel.k_pre.force.y or last_k.force.z != rel.k_pre.force.z or last_k.torque.x != rel.k_pre.torque.x or last_k.torque.y != rel.k_pre.torque.y or last_k.torque.z != rel.k_pre.torque.z):
last_k = copy.deepcopy(rel.k_pre)
print "moveImpedance(%s)"%(rel.k_pre)
velma.moveImpedance(rel.k_pre, 2.0)
if velma.checkStopCondition(2.0):
exit(0)
T_B_Wd = action.T_B_Go * rel.T_Go_Ge * rel.grasp.T_Ge_E * velma.T_E_W
duration = velma.getMovementTime(T_B_Wd, max_v_l = rel.max_v_l, max_v_r = rel.max_v_r)
velma.moveWrist2(T_B_Wd*velma.T_W_T)
raw_input("Press Enter to move the robot in " + str(duration) + " s...")
if velma.checkStopCondition():
exit(0)
velma.moveWrist(T_B_Wd, duration, Wrench(Vector3(20,20,20), Vector3(4,4,4)), abort_on_q5_singularity=True, abort_on_q5_q6_self_collision=True)
if True in rel.stop_on_contact:
# wait for contact
contacts = velma.waitForFirstContact(80, duration, emergency_stop=True, f1=True, f2=True, f3=True, palm=False)
if rel.collect_contacts:
if len(contacts) < 1:
return
for c in contacts:
self.addContact(c)
# print "found contact point"
else:
if rel.collect_contacts:
end_time = rospy.Time.now() + rospy.Duration(duration)
while rospy.Time.now() < end_time:
contacts = velma.getContactPoints(200, f1=True, f2=True, f3=True, palm=False)
for c in contacts:
self.addContact(c)
if velma.checkStopCondition(0.01):
exit(0)
if velma.aborted_on_q5_singularity:
break
else:
if velma.checkStopCondition(duration):
exit(0)
aborted = False
if velma.aborted_on_q5_singularity:
print "moving away from singularity in q_5..."
velma.moveAwayQ5Singularity(20.0/180.0*math.pi, T_B_Wd)
aborted = True
if velma.aborted_on_q5_q6_self_collision:
print "moving away from self collision in q_5 and q_6..."
closest_sector = velma.getClosestQ5Q6SpaceSector(velma.q_r[5], velma.q_r[6])
print "closest sector: %s"%(closest_sector)
q5 = (velma.q5_q6_restricted_area[closest_sector][0]+velma.q5_q6_restricted_area[closest_sector][1])/2.0
q6 = (velma.q5_q6_restricted_area[closest_sector][2]+velma.q5_q6_restricted_area[closest_sector][3])/2.0
print "q_5: %s q_6: %s"%(q5,q6)
traj, times = velma.generateTrajectoryInQ5Q6(q5,q6,5.0/180.0*math.pi)
velma.moveWrist2(traj[-1]*velma.T_W_T)
raw_input("Press Enter to move the robot in " + str(times[-1]) + " s...")
if velma.checkStopCondition():
exit(0)
velma.moveWristTraj(traj, times, Wrench(Vector3(20,20,20), Vector3(4,4,4)), abort_on_q5_singularity=False)
velma.checkStopCondition(times[-1]+0.5)
aborted = True
if aborted:
next_actions = rel.on_failure.split(',')
else:
next_actions = rel.on_success.split(',')
if rel.grasp.q != None and (last_q[0] != rel.grasp.q[0] or last_q[1] != rel.grasp.q[1] or last_q[2] != rel.grasp.q[2] or last_q[3] != rel.grasp.q[3]):
last_q = copy.deepcopy(rel.grasp.q)
velma.move_hand_client("right", rel.grasp.q, t=rel.grasp.t )
if rel.collect_contacts:
end_time = rospy.Time.now() + rospy.Duration(3.0)
while rospy.Time.now() < end_time:
contacts = velma.getContactPoints(200, f1=True, f2=True, f3=True, palm=False)
if velma.checkStopCondition(0.01):
exit(0)
for c in contacts:
self.addContact(c)
else:
if velma.checkStopCondition(3.0):
exit(0)
if rel.k_post != None and (last_k.force.x != rel.k_post.force.x or last_k.force.y != rel.k_post.force.y or last_k.force.z != rel.k_post.force.z or last_k.torque.x != rel.k_post.torque.x or last_k.torque.y != rel.k_post.torque.y or last_k.torque.z != rel.k_post.torque.z):
last_k = copy.deepcopy(rel.k_post)
print "moveImpedance(%s)"%(rel.k_post)
velma.moveImpedance(rel.k_post, 2.0)
if velma.checkStopCondition(2.0):
exit(0)
print "aborted %s next_actions: %s"%(aborted, next_actions)
for next in next_actions:
if next == "RECALCULATE":
action.recalculateRelations(self.grasps)
elif next == "FAILURE":
exit(0)
elif next == "SUCCESS":
if rel.grasp.q != None and (last_q[0] != rel.grasp.q[0] or last_q[1] != rel.grasp.q[1] or last_q[2] != rel.grasp.q[2] or last_q[3] != rel.grasp.q[3]):
last_q = copy.deepcopy(rel.grasp.q)
velma.move_hand_client("right", rel.grasp.q, t=rel.grasp.t)
if rel.collect_contacts:
end_time = rospy.Time.now() + rospy.Duration(3.0)
while rospy.Time.now() < end_time:
contacts = velma.getContactPoints(200, f1=True, f2=True, f3=True, palm=False)
if velma.checkStopCondition(0.01):
exit(0)
for c in contacts:
self.addContact(c)
else:
if velma.checkStopCondition(3.0):
exit(0)
if rel.k_post != None and (last_k.force.x != rel.k_post.force.x or last_k.force.y != rel.k_post.force.y or last_k.force.z != rel.k_post.force.z or last_k.torque.x != rel.k_post.torque.x or last_k.torque.y != rel.k_post.torque.y or last_k.torque.z != rel.k_post.torque.z):
last_k = copy.deepcopy(rel.k_post)
velma.moveImpedance(rel.k_post, 2.0)
if velma.checkStopCondition(2.0):
exit(0)
stop = True
break
elif action.getRelIndex(next) >= 0:
rel_index = action.getRelIndex(next)
break
else:
print "error: could not find relation: %s"%(next)
exit(0)
if action_index == 0:
for c in self.contacts:
jar.addContactObservation(c)
jar.processContactObservationsForTop()
jar.drawContactObservations()
self.resetContacts()
jar.resetContactObservations()
elif action_index == 1:
used_points = [False for c in self.contacts]
index = 0
for c in self.contacts:
if used_points[index]:
index += 1
continue
mean_point = PyKDL.Vector()
mean_count = 0
index_2 = 0
for d in self.contacts:
if (c-d).Norm() < 0.02:
mean_point += d
mean_count += 1
used_points[index_2] = True
index_2 += 1
if mean_count > 0:
jar.addContactObservation(mean_point*(1.0/mean_count))
# now, we have very good pose of the jar
jar.processContactObservations()
jar.drawContactObservations()
else:
contacts = velma.getContactPoints(200, f1=True, f2=True, f3=True, palm=False)
if len(contacts) > 0:
print "the jar is opened"
break
for c in self.contacts:
jar.addContactObservation(c)
jar.processContactObservationsForTop(top_offset=-0.02)
jar.drawContactObservations()
jar.resetContactObservations()
used_points = [False for c in self.contacts]
index = 0
for c in self.contacts:
if used_points[index]:
index += 1
continue
mean_point = PyKDL.Vector()
mean_count = 0
index_2 = 0
for d in self.contacts:
if (c-d).Norm() < 0.02:
mean_point += d
mean_count += 1
used_points[index_2] = True
index_2 += 1
if mean_count > 0:
jar.addContactObservation(mean_point*(1.0/mean_count))
# now, we have very good pose of the jar
jar.processContactObservations()
# jar.drawContactObservations()
action_index += 1
# jar.drawContactObservations()
rospy.sleep(2.0)
exit(0)