Traceback (most recent call last):
File "/home/zk/RAMP/test_ramp_simulation.py", line 175, in
trajectory = policy.get_action(next_obs, scene_pc)
File "/home/zk/RAMP/sim_env/ramp_policy.py", line 145, in get_action
traj_a = self.trajectory_planner.get_mppi_rollout(current_joint_angles)
File "/home/zk/RAMP/mppi_planning/trajectory_planning.py", line 287, in get_mppirollout
rollout, = self._mppi_obj.command(current_ja_tensor)
File "/home/zk/RAMP/mppi_planning/mppi_planning.py", line 246, in command
return self._command(state)
File "/home/zk/RAMP/mppi_planning/mppi_planning.py", line 289, in _command
cost_total = self._compute_total_cost_batch()
File "/home/zk/RAMP/mppi_planning/mppi_planning.py", line 382, in _compute_total_cost_batch
self.cost_total, self.states, self.actions = self._compute_rollout_costs(self.perturbed_action)
File "/home/zk/RAMP/mppi_planning/mppi_planning.py", line 354, in _compute_rollout_costs
cost_total += self._running_cost(states.view(-1, self.nx), actions.view(-1, self.nu)).view(K, T).sum(dim=1)
File "/home/zk/RAMP/mppi_planning/mppi_planning.py", line 76, in wrapper
return func(*args, *kwargs)
File "/home/zk/RAMP/mppi_planning/mppi_planning.py", line 230, in _running_cost
return self.running_cost(state, u)
File "/home/zk/RAMP/mppi_planning/trajectory_planning.py", line 132, in _mppi_running_cost
cost_collision = self._cost_collision.forward(state, self._grasped_object_nominal_control_points, self._grasped_object_grasp_T_object) if self._consider_obstacle_collisions else 0.0
File "/home/zk/RAMP/mppi_planning/cost/collision_cost.py", line 157, in forward
robot_control_points = self._get_mesh_control_points(state)
File "/home/zk/RAMP/mppi_planning/cost/collision_cost.py", line 126, in _get_mesh_control_points
link_transformations = self._differentiable_model.forward_kinematics(augmented_robot_state, end_only=False)
File "/home/zk/miniconda3/envs/ramp/lib/python3.8/site-packages/pytorch_kinematics/chain.py", line 470, in forward_kinematics
mat = super().forward_kinematics(th, frame_indices)
File "/home/zk/miniconda3/envs/ramp/lib/python3.8/site-packages/pytorch_kinematics/chain.py", line 321, in forward_kinematics
rev_jnt_transform = axis_and_angle_to_matrix_44(axes_expanded, th)
File "/home/zk/miniconda3/envs/ramp/lib/python3.8/site-packages/pytorch_kinematics/transforms/rotation_conversions.py", line 485, in axis_and_angle_to_matrix_44
rot = axis_and_angle_to_matrix_33(axis, theta)
File "/home/zk/miniconda3/envs/ramp/lib/python3.8/site-packages/pytorch_kinematics/transforms/rotation_conversions.py", line 509, in axis_and_angle_to_matrix_33
r00 = c + kx kx * one_minus_c
RuntimeError: The size of tensor a (9) must match the size of tensor b (11) at non-singleton dimension 1
Traceback (most recent call last): File "/home/zk/RAMP/test_ramp_simulation.py", line 175, in
trajectory = policy.get_action(next_obs, scene_pc)
File "/home/zk/RAMP/sim_env/ramp_policy.py", line 145, in get_action
traj_a = self.trajectory_planner.get_mppi_rollout(current_joint_angles)
File "/home/zk/RAMP/mppi_planning/trajectory_planning.py", line 287, in get_mppirollout
rollout, = self._mppi_obj.command(current_ja_tensor)
File "/home/zk/RAMP/mppi_planning/mppi_planning.py", line 246, in command
return self._command(state)
File "/home/zk/RAMP/mppi_planning/mppi_planning.py", line 289, in _command
cost_total = self._compute_total_cost_batch()
File "/home/zk/RAMP/mppi_planning/mppi_planning.py", line 382, in _compute_total_cost_batch
self.cost_total, self.states, self.actions = self._compute_rollout_costs(self.perturbed_action)
File "/home/zk/RAMP/mppi_planning/mppi_planning.py", line 354, in _compute_rollout_costs
cost_total += self._running_cost(states.view(-1, self.nx), actions.view(-1, self.nu)).view(K, T).sum(dim=1)
File "/home/zk/RAMP/mppi_planning/mppi_planning.py", line 76, in wrapper
return func(*args, *kwargs)
File "/home/zk/RAMP/mppi_planning/mppi_planning.py", line 230, in _running_cost
return self.running_cost(state, u)
File "/home/zk/RAMP/mppi_planning/trajectory_planning.py", line 132, in _mppi_running_cost
cost_collision = self._cost_collision.forward(state, self._grasped_object_nominal_control_points, self._grasped_object_grasp_T_object) if self._consider_obstacle_collisions else 0.0
File "/home/zk/RAMP/mppi_planning/cost/collision_cost.py", line 157, in forward
robot_control_points = self._get_mesh_control_points(state)
File "/home/zk/RAMP/mppi_planning/cost/collision_cost.py", line 126, in _get_mesh_control_points
link_transformations = self._differentiable_model.forward_kinematics(augmented_robot_state, end_only=False)
File "/home/zk/miniconda3/envs/ramp/lib/python3.8/site-packages/pytorch_kinematics/chain.py", line 470, in forward_kinematics
mat = super().forward_kinematics(th, frame_indices)
File "/home/zk/miniconda3/envs/ramp/lib/python3.8/site-packages/pytorch_kinematics/chain.py", line 321, in forward_kinematics
rev_jnt_transform = axis_and_angle_to_matrix_44(axes_expanded, th)
File "/home/zk/miniconda3/envs/ramp/lib/python3.8/site-packages/pytorch_kinematics/transforms/rotation_conversions.py", line 485, in axis_and_angle_to_matrix_44
rot = axis_and_angle_to_matrix_33(axis, theta)
File "/home/zk/miniconda3/envs/ramp/lib/python3.8/site-packages/pytorch_kinematics/transforms/rotation_conversions.py", line 509, in axis_and_angle_to_matrix_33
r00 = c + kx kx * one_minus_c
RuntimeError: The size of tensor a (9) must match the size of tensor b (11) at non-singleton dimension 1