robot.GetLinks()[0].GetTransform() robot.GetDOFValues() PinAndDrag.thread_params=params baselink=robot.GetLinks()[0] # Initial hrp.halfsit() config=HRP4.GetConfig(robot) p_init=robot.GetLinks()[18].GetGlobalCOM() center=baselink.GetTransform()[0:3,3] u=p_init-center R0=HRP4.rpy2mat(config[3:6]) #Prediction J=PinAndDrag.Jacobian(config[3:6],18,p_init) delta=zeros(56) delta[3:6]=1e-3*array([1,-1,2]) p_predicted=p_init+dot(J,delta) config2=config+delta R1=HRP4.rpy2mat(config2[3:6]) shift=dot(R1,u) p_predicted2=center+shift #Final HRP4.SetConfig(robot,config2)
