def test_interface_is_callable(self):
print "### interface ###"
robot = Robot()
robot.get_joint_by_id(0)
robot.get_joint_by_name("Root")
#robot.debugprint()
robot.update(PyPose())
robot.inverse_chain_t(3, (0,0,-300), 1, 100, 3)
robot.inverse_chain(3, np.array((0,0,-300)), 1, 100, 3)
robot.get_centre_of_gravity()
robot.set_angles_to_pose(PyPose(), 0, -1)
def test_maspoint_simple_r(self):
print "### Masspoint simple right ###"
config = get_config()
config["RobotTypeName"] = "Darwin"
robot = Robot()
task = KinematicTask(robot)
r_chain = task.create_cog_chain(3)
l_chain = task.create_cog_chain(4)
robot.update(PyPose())
cog=robot.get_centre_of_gravity()
lfi = robot.get_joint_by_id(35).get_chain_matrix(inverse=True)
rfi = robot.get_joint_by_id(34).get_chain_matrix(inverse=True)
rcog = np.dot(rfi, cog)
lcog = np.dot(lfi, cog)
task.update_chain(r_chain, 3)
r_cmp = r_chain.get_joint(6).get_chain_masspoint()
diff = r_cmp - rcog[0:3]
print diff
steptime = 0.005
if __name__ == "__main__":
args=sys.argv
if len(args) > 1:
play_animation(args[1])
sleep(2)
robot.update(ipc.get_pose())
#Radius in mm
factor = 45
#Kreismittelpunkt
z_offset = 0
if config["RobotTypeName"] == "Hambot":
z_offset = 300
#base_target = np.array((170, 90, -30))
r_y = robot.get_joint_by_id(r_end_joint).get_endpoint()[1]
l_y = robot.get_joint_by_id(l_end_joint).get_endpoint()[1]
r_base_target = np.array((10, r_y, -290 - z_offset))
l_base_target = np.array((10, l_y, -290 - z_offset))
r_chain = task.create_chain(root, r_end_joint, (0, 3), angle_task_joints, ignore_joints)
l_chain = task.create_chain(root, l_end_joint, (0, 3), angle_task_joints, ignore_joints)
for i in range(10000):
#local calculating stuff
begin = time.time()
current = begin
end = begin + iteration_time
dt = 0
## local stuff end
while current < end:
phase = current / float(iteration_time) * 2 * 3.14159625358
target_diff = np.array((1.5 * factor * (cos(phase)), 0, -factor * (sin(phase))))
from bitbots.robot.kinematics import Robot
from bitbots.ipc.ipc import SharedMemoryIPC
ipc = SharedMemoryIPC()
robot = Robot()
robot.update(ipc.get_pose())
if False:
"""
print 31
print numpy.dot(robot.get_joint_by_id(34).get_chain_matrix(inverse=True) , robot.get_joint_by_id(31).get_chain_matrix() )
print 36
print numpy.dot(robot.get_joint_by_id(34).get_chain_matrix(inverse=True) , robot.get_joint_by_id(36).get_chain_matrix() )
"""
print 7
print robot.get_joint_by_id(7).get_chain_matrix()
print 9
print robot.get_joint_by_id(9).get_chain_matrix()
print 11
print robot.get_joint_by_id(11).get_chain_matrix()
print 13
print robot.get_joint_by_id(13).get_chain_matrix()
print 15
print robot.get_joint_by_id(15).get_chain_matrix()
print 17
print robot.get_joint_by_id(17).get_chain_matrix()
print 34
print robot.get_joint_by_id(34).get_chain_matrix()
print 19
print robot.get_joint_by_id(19).get_chain_matrix()
