def get_transform(self,T_ref,iu_pose):
"""Get trajectory transform that would be applied to base link from
traj, and return the equivalent transform from the given starting pose
and the robot's actual base link."""
T0=mat(T_ref)
with self.robot:
#1) Copy in IU trajectory transform and format
Tc=mat(_np.eye(4))
xyzrpy=iu_pose*self.affine_signs+self.affine_offsets
#FLip yaw and roll (seems to be the best)
roll=xyzrpy[3]
pitch=xyzrpy[4]
yaw=xyzrpy[5]
xyzrpy[3]=yaw
xyzrpy[4]=pitch
xyzrpy[5]=roll
#use rodrigues function to build RPY rotation matrix
Tc[0:3,0:3]=_comps.rodrigues(self.format_angles(xyzrpy[3:6]))
Tc[0:3,3]=mat(xyzrpy[0:3]).T
#2) Set robot's 0-link at origin
self.robot.SetTransform(_np.eye(4))
#3) Find offset in current pose to desired base link (spec'd in trajectory)
T_B0=mat(self.base.GetTransform())
#4) Find relative transform to desired position
T_CB=Tc * T_B0.I
return array(T0*T_CB)
def get_transform(self,T_ref,iu_pose):
"""Get trajectory transform that would be applied to base link from
traj, and return the equivalent transform from the given starting pose
and the robot's actual base link."""
T0=mat(T_ref)
with self.robot:
#1) Copy in IU trajectory transform and format
Tc=mat(_np.eye(4))
xyzrpy=iu_pose*self.affine_signs+self.affine_offsets
#FLip yaw and roll (seems to be the best)
roll=xyzrpy[3]
pitch=xyzrpy[4]
yaw=xyzrpy[5]
xyzrpy[3]=yaw
xyzrpy[4]=pitch
xyzrpy[5]=roll
#use rodrigues function to build RPY rotation matrix
Tc[0:3,0:3]=_comps.rodrigues(self.format_angles(xyzrpy[3:6]))
Tc[0:3,3]=mat(xyzrpy[0:3]).T
#2) Set robot's 0-link at origin
self.robot.SetTransform(_np.eye(4))
#3) Find offset in current pose to desired base link (spec'd in trajectory)
T_B0=mat(self.base.GetTransform())
#4) Find relative transform to desired position
T_CB=Tc * T_B0.I
return array(T0*T_CB)
def make_robot_transform(robot):
"""hack function to get an initial pose that follows the IU ladder climbing
planner. This function will vary as the initial pose is changed."""
env=robot.GetEnv()
ladder=env.GetKinBody('ladder')
T_ladder=ladder.GetTransform()
#3rd geom is bottom rung
T_rung=ladder.GetLinks()[0].GetGeometries()[2].GetTransform()
T_rung_global=mat(T_ladder)*mat(T_rung)
#print T_rung_global
#print T_rung_global[3,1]
T0=robot.GetTransform()
T1=eye(4)
T1[0:3,0:3]=rodrigues([0,0,-pi/2])
#shift back .32 from base
T1[0:3,3]=array([-.118,.32+T_rung_global[1,3],0.002]).T
T=array(mat(T0)*mat(T1))
robot.SetTransform(T)
return T
def make_robot_transform(robot):
"""hack function to get an initial pose that follows the IU ladder climbing
planner. This function will vary as the initial pose is changed."""
env = robot.GetEnv()
ladder = env.GetKinBody('ladder')
T_ladder = ladder.GetTransform()
#3rd geom is bottom rung
T_rung = ladder.GetLinks()[0].GetGeometries()[2].GetTransform()
T_rung_global = mat(T_ladder) * mat(T_rung)
#print T_rung_global
#print T_rung_global[3,1]
T0 = robot.GetTransform()
T1 = eye(4)
T1[0:3, 0:3] = rodrigues([0, 0, -pi / 2])
#shift back .32 from base
T1[0:3, 3] = array([-.118, .32 + T_rung_global[1, 3], 0.002]).T
T = array(mat(T0) * mat(T1))
robot.SetTransform(T)
return T
