def test_createRobotFromPython(self):
self.client.robot.createRobot("test")
self.assertEqual(self.client.robot.getRobotName(), "test")
types = [
"FreeFlyer", "Planar", "RX", "RY", "RZ", "RUBX", "RUBY", "RUBZ",
"PX", "PY", "PZ", "Translation"
]
parent = ""
for i, type in enumerate(types):
jn = "joint_{}_{}".format(type, i)
bn = "body_{}_{}".format(type, i)
self.client.robot.appendJoint(parent, jn, type,
[0, 0, 0, 0, 0, 0, 1])
self.client.robot.createSphere(bn, 0.001)
self.client.robot.addObjectToJoint(jn, bn, [0, 0, 1, 0, 0, 0, 1])
parent = jn
robot = Robot(client=self.client)
n = len("JointModel")
_types = [
self.client.robot.getJointType(jn)[n:] for jn in robot.jointNames
]
self.assertListEqual(types, _types)
def resetRobot(self):
try:
self.robot = Robot(client = self.client)
except:
self.robot = None
from hpp.corbaserver.robot import Robot
Robot.packageName = 'hpp_tutorial'
Robot.urdfSuffix = ''
Robot.srdfSuffix = ''
Robot.urdfName = 'pr2'
robot = Robot('pr2', "planar")
robot.tf_root = 'base_footprint'
robot.setJointBounds("base_joint_xy", [-4, -3, -5, -3])
from hpp_ros import ScenePublisher
r = ScenePublisher(robot)
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver(robot)
q_init = robot.getCurrentConfig()
q_goal = q_init[::]
q_init[0:2] = [-3.2, -4]
rank = robot.rankInConfiguration['torso_lift_joint']
q_init[rank] = 0.2
r(q_init)
q_goal[0:2] = [-3.2, -4]
rank = robot.rankInConfiguration['l_shoulder_lift_joint']
q_goal[rank] = 0.5
rank = robot.rankInConfiguration['l_elbow_flex_joint']
q_goal[rank] = -0.5