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
def test_chain_masspoints(self):
print "### Masspoint ###"
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())
task.update_chain(l_chain, 3)
l_cmp = l_chain.get_joint(6).get_chain_masspoint()
task.update_chain(r_chain, 3)
r_cmp = r_chain.get_joint(6).get_chain_masspoint()
r_cmp[1] = -r_cmp[1]
diff = l_cmp - r_cmp
diff = np.where(diff < 0, -diff, diff)
max = diff.max()
if(not diff.max() < 1e-5):
print "Chain masspoints differ:\n%s\n%s\t%s" % (diff, l_cmp,r_cmp)
print "\n FALSE DIFF\n %s \n" % diff
import numpy as np
import time
import sys
from math import cos, sin
from bitbots.robot.kinematics import Robot, KinematicTask
from bitbots.ipc.ipc import SharedMemoryIPC
from bitbots.robot.pypose import PyPose
from bitbots.util import get_config
from bitbots.util.animation import play_animation
config = get_config()
ipc = SharedMemoryIPC()
robot = Robot()
task = KinematicTask(robot)
pose = ipc.get_pose()
#pose = PyPose()
robot.update(pose)
root = 0
r_end_joint = 34
l_end_joint = 35
angle_task_joints = [15, 16]
ignore_joints = [7, 8, 17, 18]
def update(ipc, robot, id=-1, time=0.0):
pose = ipc.get_pose()
robot.set_angles_to_pose(pose, id, time)
ipc.update(pose)
iteration_time = 0.25