def spin(self):
# load and init ik solver for right hand
velma_ikr = velmautils.VelmaIkSolver()
velma_ikr.initIkSolver()
# create Openrave interface
self.openrave = openraveinstance.OpenraveInstance(
PyKDL.Frame(PyKDL.Vector(0, 0, 0.1)))
self.openrave.startNewThread()
self.waitForOpenraveInit()
print "openrave initialised"
# create the robot interface for simulation
velma = VelmaSim(self.openrave, velma_ikr)
normals = velmautils.generateNormalsSphere(60.0 / 180.0 * math.pi)
frames = velmautils.generateFramesForNormals(60.0 / 180.0 * math.pi,
normals)
distance = 0.1
space = []
max_solutions = 0
for x in np.arange(0.0, 1.01, distance):
for y in np.arange(-1.0, 1.01, distance):
for z in np.arange(1.0, 2.01, distance):
solutions = 0
for fr in frames:
T_Br_E = PyKDL.Frame(PyKDL.Vector(x, y, z)) * fr
if self.openrave.findIkSolution(T_Br_E) != None:
solutions += 1
space.append([x, y, z, solutions])
if solutions > max_solutions:
max_solutions = solutions
print "x: %s" % (x)
print "points: %s" % (len(space))
m_id = 0
for s in space:
value = float(s[3]) / float(max_solutions)
m_id = publishSinglePointMarker(
PyKDL.Vector(s[0], s[1], s[2]),
m_id,
r=1,
g=value,
b=value,
namespace='default',
frame_id='world',
m_type=Marker.SPHERE,
scale=Vector3(distance * value, distance * value,
distance * value),
T=None)
if m_id % 100 == 0:
rospy.sleep(0.1)
print "done"
return
def spin(self):
# load and init ik solver for right hand
velma_ikr = velmautils.VelmaIkSolver()
velma_ikr.initIkSolver()
# create Openrave interface
self.openrave = openraveinstance.OpenraveInstance(PyKDL.Frame(PyKDL.Vector(0,0,0.1)))
self.openrave.startNewThread()
self.waitForOpenraveInit()
print "openrave initialised"
# create the robot interface for simulation
velma = VelmaSim(self.openrave, velma_ikr)
normals = velmautils.generateNormalsSphere(60.0/180.0*math.pi)
frames = velmautils.generateFramesForNormals(60.0/180.0*math.pi, normals)
distance = 0.1
space = []
max_solutions = 0
for x in np.arange(0.0, 1.01, distance):
for y in np.arange(-1.0, 1.01, distance):
for z in np.arange(1.0, 2.01, distance):
solutions = 0
for fr in frames:
T_Br_E = PyKDL.Frame(PyKDL.Vector(x,y,z)) * fr
if self.openrave.findIkSolution(T_Br_E) != None:
solutions += 1
space.append([x,y,z,solutions])
if solutions > max_solutions:
max_solutions = solutions
print "x: %s"%(x)
print "points: %s"%(len(space))
m_id = 0
for s in space:
value = float(s[3]) / float(max_solutions)
m_id = publishSinglePointMarker(PyKDL.Vector(s[0], s[1], s[2]), m_id, r=1, g=value, b=value, namespace='default', frame_id='world', m_type=Marker.SPHERE, scale=Vector3(distance*value, distance*value, distance*value), T=None)
if m_id%100 == 0:
rospy.sleep(0.1)
print "done"
return
def spin(self):
if True:
# Create a world object
world = ode.World()
# world.setGravity( (0,0,-3.81) )
world.setGravity( (0,0,0) )
CFM = world.getCFM()
ERP = world.getERP()
print "CFM: %s ERP: %s"%(CFM, ERP)
# world.setCFM(0.001)
# print "CFM: %s ERP: %s"%(CFM, ERP)
m_id = 0
self.pub_marker.eraseMarkers(0,3000, frame_id='world')
#
# Init Openrave
#
parser = OptionParser(description='Openrave Velma interface')
OpenRAVEGlobalArguments.addOptions(parser)
(options, leftargs) = parser.parse_args()
self.env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True)
# self.openrave_robot = self.env.ReadRobotXMLFile('robots/barretthand_ros.robot.xml')
mimic_joints = [
("right_HandFingerTwoKnuckleOneJoint", "right_HandFingerOneKnuckleOneJoint*1.0", "|right_HandFingerOneKnuckleOneJoint 1.0", ""),
("right_HandFingerOneKnuckleThreeJoint", "right_HandFingerOneKnuckleTwoJoint*0.33333", "|right_HandFingerOneKnuckleTwoJoint 0.33333", ""),
("right_HandFingerTwoKnuckleThreeJoint", "right_HandFingerTwoKnuckleTwoJoint*0.33333", "|right_HandFingerTwoKnuckleTwoJoint 0.33333", ""),
("right_HandFingerThreeKnuckleThreeJoint", "right_HandFingerThreeKnuckleTwoJoint*0.33333", "|right_HandFingerThreeKnuckleTwoJoint 0.33333", ""),
]
rospack = rospkg.RosPack()
self.urdf_module = RaveCreateModule(self.env, 'urdf')
xacro_uri = rospack.get_path('barrett_hand_defs') + '/robots/barrett_hand.urdf.xml'
urdf_uri = '/tmp/barrett_hand.urdf'
srdf_uri = rospack.get_path('barrett_hand_defs') + '/robots/barrett_hand.srdf'
arg1 = "collision_model_full:=true"
arg2 = "collision_model_simplified:=false"
arg3 = "collision_model_enlargement:=0.0"
arg4 = "collision_model_no_hands:=false"
subprocess.call(["rosrun", "xacro", "xacro", "-o", urdf_uri, xacro_uri, arg1, arg2, arg3, arg4])
robot_name = self.urdf_module.SendCommand('load ' + urdf_uri + ' ' + srdf_uri )
print "robot name: " + robot_name
self.openrave_robot = self.env.GetRobot(robot_name)
self.env.Remove(self.openrave_robot)
for mimic in mimic_joints:
mj = self.openrave_robot.GetJoint(mimic[0])
mj.SetMimicEquations(0, mimic[1], mimic[2], mimic[3])
self.openrave_robot.GetActiveManipulator().SetChuckingDirection([0,0,0,0])#directions)
self.env.Add(self.openrave_robot)
self.openrave_robot = self.env.GetRobot(robot_name)
# print "robots: "
# print self.env.GetRobots()
joint_names = []
print "active joints:"
for j in self.openrave_robot.GetJoints():
joint_names.append(j.GetName())
print j
print "passive joints:"
for j in self.openrave_robot.GetPassiveJoints():
joint_names.append(j.GetName())
print j
# ODE does not support distance measure
self.env.GetCollisionChecker().SetCollisionOptions(CollisionOptions.Contacts)
self.env.Add(self.openrave_robot)
vertices, faces = surfaceutils.readStl(rospack.get_path('velma_scripts') + "/data/meshes/klucz_gerda_ascii.stl", scale=1.0)
# vertices, faces = surfaceutils.readStl("klucz_gerda_ascii.stl", scale=1.0)
self.addTrimesh("object", vertices, faces)
# self.addBox("object", 0.2,0.06,0.06)
# self.addSphere("object", 0.15)
# vertices, faces = self.getMesh("object")
#
# definition of the expected external wrenches
#
ext_wrenches = []
# origin of the external wrench (the end point of the key)
wr_orig = PyKDL.Vector(0.039, 0.0, 0.0)
for i in range(8):
# expected force at the end point
force = PyKDL.Frame(PyKDL.Rotation.RotX(float(i)/8.0 * 2.0 * math.pi)) * PyKDL.Vector(0,1,0)
# expected torque at the com
torque = wr_orig * force
ext_wrenches.append(PyKDL.Wrench(force, torque))
# expected force at the end point
force = PyKDL.Frame(PyKDL.Rotation.RotX(float(i)/8.0 * 2.0 * math.pi)) * PyKDL.Vector(-1,1,0)
# expected torque at the com
torque = wr_orig * force
ext_wrenches.append(PyKDL.Wrench(force, torque))
#
# definition of the grasps
#
grasp_id = 0
if grasp_id == 0:
grasp_direction = [0, 1, -1, 1] # spread, f1, f3, f2
grasp_initial_configuration = [60.0/180.0*math.pi, None, None, None]
self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.122103662206, -0.124395758838, -0.702726011729, 0.689777190329), PyKDL.Vector(-0.00115787237883, -0.0194999426603, 0.458197712898))
# self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.174202588426, -0.177472708083, -0.691231954612, 0.678495061771), PyKDL.Vector(0.0, -0.0213436260819, 0.459123969078))
elif grasp_id == 1:
grasp_direction = [0, 1, -1, 1] # spread, f1, f3, f2
grasp_initial_configuration = [90.0/180.0*math.pi, None, None, None]
self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.0187387771868, -0.708157209758, -0.0317875569224, 0.705090018033), PyKDL.Vector(4.65661287308e-10, 0.00145332887769, 0.472836345434))
elif grasp_id == 2:
grasp_direction = [0, 1, 0, 0] # spread, f1, f3, f2
grasp_initial_configuration = [90.0/180.0*math.pi, None, 90.0/180.0*math.pi, 0]
self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.0187387763947, -0.708157179826, -0.0317875555789, 0.705089928626), PyKDL.Vector(0.0143095180392, 0.00145332887769, 0.483659058809))
elif grasp_id == 3:
grasp_direction = [0, 1, 1, 1] # spread, f1, f3, f2
grasp_initial_configuration = [90.0/180.0*math.pi, None, None, None]
self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.00518634245761, -0.706548316769, -0.0182458505507, 0.707410947861), PyKDL.Vector(0.000126354629174, -0.00217361748219, 0.47637796402))
elif grasp_id == 4:
grasp_direction = [0, 0, 1, 0] # spread, f1, f3, f2
grasp_initial_configuration = [90.0/180.0*math.pi, 100.0/180.0*math.pi, None, 100.0/180.0*math.pi]
self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.153445252933, -0.161230275653, 0.681741576082, 0.696913201022), PyKDL.Vector(0.000126355327666, 0.00152841210365, 0.466048002243))
elif grasp_id == 5:
grasp_direction = [0, 0, 1, 0] # spread, f1, f3, f2
grasp_initial_configuration = [100.0/180.0*math.pi, 101.5/180.0*math.pi, None, 101.5/180.0*math.pi]
self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.155488650062, -0.159260521271, 0.690572597636, 0.688163302213), PyKDL.Vector(-0.000278688268736, 0.00575117766857, 0.461560428143))
elif grasp_id == 6:
grasp_direction = [0, 1, -1, 1] # spread, f1, f3, f2
grasp_initial_configuration = [90.0/180.0*math.pi, None, 0, 0]
self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.512641041738, -0.485843507183, -0.514213889193, 0.48655882699), PyKDL.Vector(-0.000278423947748, -0.00292747467756, 0.445628076792))
else:
print "ERROR: unknown grasp_id: %s"%(grasp_id)
exit(0)
self.updatePose("object", self.T_W_O)
with open('barret_hand_openrave2ros_joint_map2.txt', 'r') as f:
lines = f.readlines()
joint_map = {}
for line in lines:
line_s = line.split()
if len(line_s) == 2:
joint_map[line_s[0]] = line_s[1]
elif len(line_s) != 1:
print "error in joint map file"
exit(0)
print joint_map
self.pub_js = rospy.Publisher("/joint_states", JointState)
if True:
def getDirectionIndex(n):
min_angle = -45.0/180.0*math.pi
angle_range = 90.0/180.0*math.pi
angles_count = 5
angles_count2 = angles_count * angles_count
max_indices = angles_count2 * 6
if abs(n.x()) > abs(n.y()) and abs(n.x()) > abs(n.z()):
if n.x() > 0:
sec = 0
a1 = math.atan2(n.y(), n.x())
a2 = math.atan2(n.z(), n.x())
else:
sec = 1
a1 = math.atan2(n.y(), -n.x())
a2 = math.atan2(n.z(), -n.x())
elif abs(n.y()) > abs(n.x()) and abs(n.y()) > abs(n.z()):
if n.y() > 0:
sec = 2
a1 = math.atan2(n.x(), n.y())
a2 = math.atan2(n.z(), n.y())
else:
sec = 3
a1 = math.atan2(n.x(), -n.y())
a2 = math.atan2(n.z(), -n.y())
else:
if n.z() > 0:
sec = 4
a1 = math.atan2(n.x(), n.z())
a2 = math.atan2(n.y(), n.z())
else:
sec = 5
a1 = math.atan2(n.x(), -n.z())
a2 = math.atan2(n.y(), -n.z())
a1i = int(angles_count*(a1-min_angle)/angle_range)
a2i = int(angles_count*(a2-min_angle)/angle_range)
if a1i < 0:
print sec, a1i, a2i
a1i = 0
if a1i >= angles_count:
# print sec, a1i, a2i
a1i = angles_count-1
if a2i < 0:
print sec, a1i, a2i
a2i = 0
if a2i >= angles_count:
print sec, a1i, a2i
a2i = angles_count-1
return sec * angles_count2 + a1i * angles_count + a2i
# generate a dictionary of indexed directions
normals_sphere_indexed_dir = velmautils.generateNormalsSphere(3.0/180.0*math.pi)
print len(normals_sphere_indexed_dir)
indices_directions = {}
for n in normals_sphere_indexed_dir:
index = getDirectionIndex(n)
if not index in indices_directions:
indices_directions[index] = [n]
else:
indices_directions[index].append(n)
for index in indices_directions:
n_mean = PyKDL.Vector()
for n in indices_directions[index]:
n_mean += n
n_mean.Normalize()
indices_directions[index] = n_mean
# test direction indexing
if False:
normals_sphere = velmautils.generateNormalsSphere(3.0/180.0*math.pi)
print len(normals_sphere)
m_id = 0
for current_index in range(5*5*6):
count = 0
r = random.random()
g = random.random()
b = random.random()
for n in normals_sphere:
sec = -1
a1 = None
a2 = None
index = getDirectionIndex(n)
if index == current_index:
m_id = self.pub_marker.publishSinglePointMarker(n*0.1, m_id, r=r, g=g, b=b, namespace='default', frame_id='world', m_type=Marker.CUBE, scale=Vector3(0.007, 0.007, 0.007), T=None)
count += 1
print current_index, count
for index in indices_directions:
m_id = self.pub_marker.publishSinglePointMarker(indices_directions[index]*0.12, m_id, r=1, g=1, b=1, namespace='default', frame_id='world', m_type=Marker.CUBE, scale=Vector3(0.012, 0.012, 0.012), T=None)
exit(0)
#
# PyODE test
#
if True:
fixed_joints_names_for_fixed_DOF = [
["right_HandFingerOneKnuckleOneJoint", "right_HandFingerTwoKnuckleOneJoint"], # spread
["right_HandFingerOneKnuckleTwoJoint", "right_HandFingerOneKnuckleThreeJoint"], # f1
["right_HandFingerThreeKnuckleTwoJoint", "right_HandFingerThreeKnuckleThreeJoint"], # f3
["right_HandFingerTwoKnuckleTwoJoint", "right_HandFingerTwoKnuckleThreeJoint"], # f2
]
coupled_joint_names_for_fingers = ["right_HandFingerOneKnuckleThreeJoint", "right_HandFingerTwoKnuckleThreeJoint", "right_HandFingerThreeKnuckleThreeJoint"]
actuated_joints_for_DOF = [
["right_HandFingerOneKnuckleOneJoint", "right_HandFingerTwoKnuckleOneJoint"], # spread
["right_HandFingerOneKnuckleTwoJoint"], # f1
["right_HandFingerThreeKnuckleTwoJoint"], # f3
["right_HandFingerTwoKnuckleTwoJoint"]] # f2
ignored_links = ["world", "world_link"]
self.run_ode_simulation = False
self.insert6DofGlobalMarker(self.T_W_O)
print "grasp_direction = [%s, %s, %s, %s] # spread, f1, f3, f2"%(grasp_direction[0], grasp_direction[1], grasp_direction[2], grasp_direction[3])
print "grasp_initial_configuration = [%s, %s, %s, %s]"%(grasp_initial_configuration[0], grasp_initial_configuration[1], grasp_initial_configuration[2], grasp_initial_configuration[3])
print "grasp_final_configuration = %s"%(self.openrave_robot.GetDOFValues())
rot_q = self.T_W_O.M.GetQuaternion()
print "self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(%s, %s, %s, %s), PyKDL.Vector(%s, %s, %s))"%(rot_q[0], rot_q[1], rot_q[2], rot_q[3], self.T_W_O.p.x(), self.T_W_O.p.y(), self.T_W_O.p.z())
self.erase6DofMarker()
raw_input("Press ENTER to continue...")
# reset the gripper in Openrave
self.openrave_robot.SetDOFValues([0,0,0,0])
# read the position of all links
grasp_links_poses = {}
for link in self.openrave_robot.GetLinks():
grasp_links_poses[link.GetName()] = self.OpenraveToKDL(link.GetTransform())
# print "obtained the gripper configuration for the grasp:"
# print finalconfig[0]
#
# simulation in ODE
#
# Create a world object
world = ode.World()
# world.setGravity( (0,0,-3.81) )
world.setGravity( (0,0,0) )
CFM = world.getCFM()
ERP = world.getERP()
print "CFM: %s ERP: %s"%(CFM, ERP)
# world.setCFM(0.001)
# print "CFM: %s ERP: %s"%(CFM, ERP)
self.space = ode.Space()
# Create a body inside the world
body = ode.Body(world)
M = ode.Mass()
M.setCylinderTotal(0.02, 1, 0.005, 0.09)
body.setMass(M)
ode_mesh = ode.TriMeshData()
ode_mesh.build(vertices, faces)
geom = ode.GeomTriMesh(ode_mesh, self.space)
geom.name = "object"
geom.setBody(body)
self.setOdeBodyPose(geom, self.T_W_O)
ode_gripper_geoms = {}
for link in self.openrave_robot.GetLinks():
link_name = link.GetName()
if link_name in ignored_links:
print "ignoring: %s"%(link_name)
continue
print "adding: %s"%(link_name)
ode_mesh_link = None
body_link = None
T_W_L = self.OpenraveToKDL(link.GetTransform())
col = link.GetCollisionData()
vertices = col.vertices
faces = col.indices
ode_mesh_link = ode.TriMeshData()
ode_mesh_link.build(vertices, faces)
ode_gripper_geoms[link_name] = ode.GeomTriMesh(ode_mesh_link, self.space)
if True:
body_link = ode.Body(world)
M_link = ode.Mass()
M_link.setCylinderTotal(0.05, 1, 0.01, 0.09)
body_link.setMass(M_link)
ode_gripper_geoms[link_name].setBody(body_link)
ode_gripper_geoms[link_name].name = link.GetName()
self.setOdeBodyPose(body_link, T_W_L)
actuated_joint_names = []
for dof in range(4):
for joint_name in actuated_joints_for_DOF[dof]:
actuated_joint_names.append(joint_name)
ode_gripper_joints = {}
for joint_name in joint_names:
joint = self.openrave_robot.GetJoint(joint_name)
link_parent = joint.GetHierarchyParentLink().GetName()
link_child = joint.GetHierarchyChildLink().GetName()
if link_parent in ode_gripper_geoms and link_child in ode_gripper_geoms:
axis = joint.GetAxis()
anchor = joint.GetAnchor()
if joint_name in actuated_joint_names:
ode_gripper_joints[joint_name] = ode.AMotor(world)
ode_gripper_joints[joint_name].attach(ode_gripper_geoms[link_parent].getBody(), ode_gripper_geoms[link_child].getBody())
ode_gripper_joints[joint_name].setMode(ode.AMotorUser)#Euler)
# ode_gripper_joints[joint_name].setNumAxes(3)
ode_gripper_joints[joint_name].setAxis(0,1,axis)
# ode_gripper_joints[joint_name].setAxis(1,1,axis)
# ode_gripper_joints[joint_name].setAxis(2,1,axis)
ode_gripper_joints[joint_name].setParam(ode.ParamFMax, 10.0)
# ode_gripper_joints[joint_name].setParam(ode.ParamFMax2, 10.0)
# ode_gripper_joints[joint_name].setParam(ode.ParamFMax3, 10.0)
else:
ode_gripper_joints[joint_name] = ode.HingeJoint(world)
ode_gripper_joints[joint_name].attach(ode_gripper_geoms[link_parent].getBody(), ode_gripper_geoms[link_child].getBody())
ode_gripper_joints[joint_name].setAxis(-axis)
ode_gripper_joints[joint_name].setAnchor(anchor)
# ode_gripper_joints[joint_name] = ode.HingeJoint(world)
# ode_gripper_joints[joint_name].attach(ode_gripper_geoms[link_parent].getBody(), ode_gripper_geoms[link_child].getBody())
# ode_gripper_joints[joint_name].setAxis(-axis)
# ode_gripper_joints[joint_name].setAnchor(anchor)
# if joint_name in actuated_joint_names:
# ode_gripper_joints[joint_name].setParam(ode.ParamFMax, 10.0)
limits = joint.GetLimits()
value = joint.GetValue(0)
# print joint_name
# print "limits: %s %s"%(limits[0], limits[1])
# print "axis: %s"%(axis)
# print "anchor: %s"%(anchor)
# print "value: %s"%(value)
lim = [limits[0], limits[1]]
if limits[0] <= -math.pi:
# print "lower joint limit %s <= -PI, setting to -PI+0.01"%(limits[0])
lim[0] = -math.pi + 0.01
if limits[1] >= math.pi:
# print "upper joint limit %s >= PI, setting to PI-0.01"%(limits[1])
lim[1] = math.pi - 0.01
ode_gripper_joints[joint_name].setParam(ode.ParamLoStop, lim[0])
ode_gripper_joints[joint_name].setParam(ode.ParamHiStop, lim[1])
# ode_gripper_joints[joint_name].setParam(ode.ParamFudgeFactor, 0.5)
ode_fixed_joint = ode.FixedJoint(world)
ode_fixed_joint.attach(None, ode_gripper_geoms["right_HandPalmLink"].getBody())
ode_fixed_joint.setFixed()
#
# set the poses of all links as for the grasp
#
for link_name in grasp_links_poses:
if link_name in ignored_links:
continue
ode_body = ode_gripper_geoms[link_name].getBody()
T_W_L = grasp_links_poses[link_name]
self.setOdeBodyPose(ode_body, T_W_L)
fixed_joint_names = []
fixed_joint_names += coupled_joint_names_for_fingers
for dof in range(4):
if grasp_direction[dof] == 0.0:
for joint_name in fixed_joints_names_for_fixed_DOF[dof]:
if not joint_name in fixed_joint_names:
fixed_joint_names.append(joint_name)
#
# change all coupled joints to fixed joints
#
fixed_joints = {}
for joint_name in fixed_joint_names:
# save the bodies attached
body0 = ode_gripper_joints[joint_name].getBody(0)
body1 = ode_gripper_joints[joint_name].getBody(1)
# save the joint angle
if joint_name in actuated_joint_names:
angle = ode_gripper_joints[joint_name].getAngle(0)
else:
angle = ode_gripper_joints[joint_name].getAngle()
# detach the joint
ode_gripper_joints[joint_name].attach(None, None)
del ode_gripper_joints[joint_name]
fixed_joints[joint_name] = [ode.FixedJoint(world), angle]
fixed_joints[joint_name][0].attach(body0, body1)
fixed_joints[joint_name][0].setFixed()
# change all actuated joints to motor joints
# actuated_joint_names = []
# for dof in range(4):
# for joint_name in actuated_joints_for_DOF[dof]:
# actuated_joint_names.append(joint_name)
# for joint_name in actuated_joint_names:
# A joint group for the contact joints that are generated whenever
# two bodies collide
contactgroup = ode.JointGroup()
print "ode_gripper_geoms:"
print ode_gripper_geoms
initial_T_W_O = self.T_W_O
# Do the simulation...
dt = 0.001
total_time = 0.0
failure = False
#
# PID
#
Kc = 1.0
# Ti = 1000000000000.0
KcTi = 0.0
Td = 0.0
Ts = 0.001
pos_d = 1.0
e0 = 0.0
e1 = 0.0
e2 = 0.0
u = 0.0
u_max = 1.0
while total_time < 5.0 and not rospy.is_shutdown():
#
# ODE simulation
#
joint = ode_gripper_joints["right_HandFingerOneKnuckleTwoJoint"]
e2 = e1
e1 = e0
e0 = pos_d - joint.getAngle(0)
u = u + Kc * (e0 - e1) + KcTi * Ts * e0 + Kc * Td / Ts * (e0 - 2.0 * e1 + e2)
if u > u_max:
u = u_max
if u < -u_max:
u = -u_max
joint.setParam(ode.ParamFMax, 10000.0)
joint.setParam(ode.ParamVel, 1.1)
# joint.setParam(ode.ParamVel2, 1.1)
# joint.setParam(ode.ParamVel3, 1.1)
print joint.getAngle(0), " ", u #, " ", joint.getAngleRate(0)
# print u
# joint.addTorque(u)
# for dof in range(4):
# for joint_name in actuated_joints_for_DOF[dof]:
# if joint_name in ode_gripper_joints:
# ode_gripper_joints[joint_name].addTorque(1*grasp_direction[dof])
self.grasp_contacts = []
self.space.collide((world,contactgroup), self.near_callback)
world.step(dt)
total_time += dt
contactgroup.empty()
#
# ROS interface
#
old_m_id = m_id
m_id = 0
# publish frames from ODE
if False:
for link_name in ode_gripper_geoms:
link_body = ode_gripper_geoms[link_name].getBody()
if link_body == None:
link_body = ode_gripper_geoms[link_name]
T_W_Lsim = self.getOdeBodyPose(link_body)
m_id = self.pub_marker.publishFrameMarker(T_W_Lsim, m_id, scale=0.05, frame='world', namespace='default')
# publish the mesh of the object
T_W_Osim = self.getOdeBodyPose(body)
m_id = self.pub_marker.publishConstantMeshMarker("package://velma_scripts/data/meshes/klucz_gerda_binary.stl", m_id, r=1, g=0, b=0, scale=1.0, frame_id='world', namespace='default', T=T_W_Osim)
# update the gripper visualization in ros
js = JointState()
js.header.stamp = rospy.Time.now()
for jn in joint_map:
ros_joint_name = joint_map[jn]
js.name.append(ros_joint_name)
if jn in ode_gripper_joints:
if jn in actuated_joint_names:
js.position.append( ode_gripper_joints[jn].getAngle(0) )
else:
js.position.append( ode_gripper_joints[jn].getAngle() )
elif jn in fixed_joints:
js.position.append(fixed_joints[jn][1])
else:
js.position.append(0)
self.pub_js.publish(js)
# draw contacts
for c in self.grasp_contacts:
cc = PyKDL.Vector(c[0], c[1], c[2])
cn = PyKDL.Vector(c[3], c[4], c[5])
m_id = self.pub_marker.publishVectorMarker(cc, cc+cn*0.04, m_id, 1, 0, 0, frame='world', namespace='default', scale=0.003)
if m_id < old_m_id:
self.pub_marker.eraseMarkers(m_id,old_m_id+1, frame_id='world')
diff_T_W_O = PyKDL.diff(initial_T_W_O, T_W_Osim)
# if diff_T_W_O.vel.Norm() > 0.02 or diff_T_W_O.rot.Norm() > 30.0/180.0*math.pi:
# print "the object moved"
# print diff_T_W_O
# failure = True
# break
rospy.sleep(0.01)
if not failure:
T_O_Wsim = T_W_Osim.Inverse()
TR_O_Wsim = PyKDL.Frame(T_O_Wsim.M)
contacts = []
for c in self.grasp_contacts:
cc_W = PyKDL.Vector(c[0], c[1], c[2])
cn_W = PyKDL.Vector(c[3], c[4], c[5])
cc_O = T_O_Wsim * cc_W
cn_O = TR_O_Wsim * cn_W
contacts.append([cc_O[0], cc_O[1], cc_O[2], cn_O[0], cn_O[1], cn_O[2]])
gws, contact_planes = self.generateGWS(contacts, 1.0)
grasp_quality = None
for wr in ext_wrenches:
wr_qual = self.getQualityMeasure2(gws, wr)
if grasp_quality == None or wr_qual < grasp_quality:
grasp_quality = wr_qual
grasp_quality_classic = self.getQualityMeasure(gws)
print "grasp_quality_classic: %s grasp_quality: %s"%(grasp_quality_classic, grasp_quality)
exit(0)
def __init__(self, vol_radius, vol_samples_count, T_H_O, orientations_angle, vertices_obj, faces_obj):
self.vol_radius = vol_radius
self.vol_samples_count = vol_samples_count
self.index_factor = float(self.vol_samples_count)/(2.0*self.vol_radius)
self.vol_samples = []
for x in np.linspace(-self.vol_radius, self.vol_radius, self.vol_samples_count):
self.vol_samples.append([])
for y in np.linspace(-self.vol_radius, self.vol_radius, self.vol_samples_count):
self.vol_samples[-1].append([])
for z in np.linspace(-self.vol_radius, self.vol_radius, self.vol_samples_count):
self.vol_samples[-1][-1].append([])
self.vol_samples[-1][-1][-1] = {}
self.vol_sample_points = []
for xi in range(self.vol_samples_count):
for yi in range(self.vol_samples_count):
for zi in range(self.vol_samples_count):
self.vol_sample_points.append( self.getVolPoint(xi,yi,zi) )
self.T_H_O = T_H_O
self.T_O_H = self.T_H_O.Inverse()
# generate the set of orientations
self.orientations_angle = orientations_angle
normals_sphere = velmautils.generateNormalsSphere(self.orientations_angle)
orientations1 = velmautils.generateFramesForNormals(self.orientations_angle, normals_sphere)
orientations2 = []
for ori in orientations1:
x_axis = ori * PyKDL.Vector(1,0,0)
if x_axis.z() > 0.0:
orientations2.append(ori)
self.orientations = {}
for ori_idx in range(len(orientations2)):
self.orientations[ori_idx] = orientations2[ori_idx]
# generate a set of surface points
self.surface_points_obj = surfaceutils.sampleMeshDetailedRays(vertices_obj, faces_obj, 0.0015)
print "surface of the object has %s points"%(len(self.surface_points_obj))
# disallow contact with the surface points beyond the key handle
for p in self.surface_points_obj:
if p.pos.x() > 0.0:
p.allowed = False
surface_points_obj_init = []
surface_points2_obj_init = []
for sp in self.surface_points_obj:
if sp.allowed:
surface_points_obj_init.append(sp)
else:
surface_points2_obj_init.append(sp)
print "generating a subset of surface points of the object..."
while True:
p_idx = random.randint(0, len(self.surface_points_obj)-1)
if self.surface_points_obj[p_idx].allowed:
break
p_dist = 0.003
self.sampled_points_obj = []
while True:
self.sampled_points_obj.append(p_idx)
surface_points_obj2 = []
for sp in surface_points_obj_init:
if (sp.pos-self.surface_points_obj[p_idx].pos).Norm() > p_dist:
surface_points_obj2.append(sp)
if len(surface_points_obj2) == 0:
break
surface_points_obj_init = surface_points_obj2
p_idx = surface_points_obj_init[0].id
print "subset size: %s"%(len(self.sampled_points_obj))
print "generating a subset of other surface points of the object..."
p_dist2 = 0.006
while True:
p_idx = random.randint(0, len(self.surface_points_obj)-1)
if not self.surface_points_obj[p_idx].allowed:
break
self.sampled_points2_obj = []
while True:
self.sampled_points2_obj.append(p_idx)
surface_points2_obj2 = []
for sp in surface_points2_obj_init:
if (sp.pos-self.surface_points_obj[p_idx].pos).Norm() > p_dist2:
surface_points2_obj2.append(sp)
if len(surface_points2_obj2) == 0:
break
surface_points2_obj_init = surface_points2_obj2
p_idx = surface_points2_obj_init[0].id
# test volumetric model
if False:
for pt_idx in self.sampled_points2_obj:
pt = self.surface_points_obj[pt_idx]
m_id = self.pub_marker.publishSinglePointMarker(pt.pos, m_id, r=1, g=0, b=0, namespace='default', frame_id='world', m_type=Marker.CUBE, scale=Vector3(0.003, 0.003, 0.003), T=None)
rospy.sleep(0.001)
for pt_idx in self.sampled_points_obj:
pt = self.surface_points_obj[pt_idx]
m_id = self.pub_marker.publishSinglePointMarker(pt.pos, m_id, r=0, g=1, b=0, namespace='default', frame_id='world', m_type=Marker.CUBE, scale=Vector3(0.003, 0.003, 0.003), T=None)
rospy.sleep(0.001)
print "subset size: %s"%(len(self.sampled_points2_obj))
print "calculating surface curvature at sampled points of the obj..."
m_id = 0
planes = 0
edges = 0
points = 0
for pt_idx in range(len(self.surface_points_obj)):
indices, nx, pc1, pc2 = surfaceutils.pclPrincipalCurvaturesEstimation(self.surface_points_obj, pt_idx, 5, 0.003)
# m_id = self.pub_marker.publishVectorMarker(self.T_W_O * self.surface_points_obj[pt_idx].pos, self.T_W_O * (self.surface_points_obj[pt_idx].pos + nx*0.004), m_id, 1, 0, 0, frame='world', namespace='default', scale=0.0002)
# m_id = self.pub_marker.publishVectorMarker(self.T_W_O * self.surface_points_obj[pt_idx].pos, self.T_W_O * (self.surface_points_obj[pt_idx].pos + self.surface_points_obj[pt_idx].normal*0.004), m_id, 0, 0, 1, frame='world', namespace='default', scale=0.0002)
self.surface_points_obj[pt_idx].frame = PyKDL.Frame(PyKDL.Rotation(nx, self.surface_points_obj[pt_idx].normal * nx, self.surface_points_obj[pt_idx].normal), self.surface_points_obj[pt_idx].pos)
self.surface_points_obj[pt_idx].pc1 = pc1
self.surface_points_obj[pt_idx].pc2 = pc2
if pc1 < 0.2:
self.surface_points_obj[pt_idx].is_plane = True
self.surface_points_obj[pt_idx].is_edge = False
self.surface_points_obj[pt_idx].is_point = False
planes += 1
elif pc2 < 0.2:
self.surface_points_obj[pt_idx].is_plane = False
self.surface_points_obj[pt_idx].is_edge = True
self.surface_points_obj[pt_idx].is_point = False
edges += 1
else:
self.surface_points_obj[pt_idx].is_plane = False
self.surface_points_obj[pt_idx].is_edge = False
self.surface_points_obj[pt_idx].is_point = True
points += 1
print "obj planes: %s edges: %s points: %s"%(planes, edges, points)
